Arduinoで三軸ジャイロセンサーと測距レーザを両立させるにはどうすればいいか

前提・実現したいこと

現在、Arduinoで、三軸ジャイロセンサーと測距レーザの二つを利用する装置を作成しています。具体的には、装置の高さをあらかじめ決定し、三軸ジャイロセンサーで測定した角度を利用し、三角関数で距離を求めます。そして測距レーザで実際求めた距離とどれ位差があるかを求めるというものです。

試したこと・問題

ひとつのプログラミングにまとめたのですが、三軸ジャイロセンサーは瞬間加速度で角度を求めるプログラミングで、測距レーザは平均を出して距離を求めるプログラミングとなっているため、相性が非常に悪く、角度を正確に出そうとすると、レーザで算出した値を平均する総数を減らさないといけず、距離が正確に測れなくなってしまいました。そして、距離を正確に出そうとすると、平均の総数を増やさなくてはならず、そうすると、処理がおちてしまい、角速度を正確に測ることができず、角度を求めることができなくなってしまいました。この二つを両立させるにはどうすればいいでしょうか。
また、両立させることは不可能でしょうか。

該当のソースコード

ひとつにまとめたプログラミングを記述します。短くする方法がわからないのでこのままで失礼します。

#include<MsTimer2.h>
#include<Wire.h>

//変数定義
#define INTERVAL 10
#define INT_MAX 32767
#define ANGLE_RATE_MAX 500
#define BUFF_SIZE 128

const byte L3GD20_ADDR = B1101010;
const byte L3GD20_WHOAMI = 0x0f;
const byte L3GD20_CTRL1 = 0x20;
const byte L3GD20_CTRL2 = 0x21;
const byte L3GD20_CTRL3 = 0x22;
const byte L3GD20_CTRL4 = 0x23;
const byte L3GD20_CTRL5 = 0x24;
const byte L3GD20_X_L = 0x28; const byte L3GD20_X_H = 0x29;
const byte L3GD20_Y_L = 0x2A; const byte L3GD20_Y_H = 0x2B;
const byte L3GD20_Z_L = 0x2C; const byte L3GD20_Z_H = 0x2D;

int count;
int GyroBuff[BUFF_SIZE];
double Bias;
int Backup_X =0,Backup_Y=0,Backup_Z= 0;
double anglex =0.0,angley =0.0,anglez= 0.0;
boolean gyoroGo = false;

//タイマー割り込み
void clock(void){
  gyoroGo=true;
  return;
}

void L3GD20_write(byte reg, byte val)
{
  Wire.beginTransmission(L3GD20_ADDR);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission();
}

byte L3GD20_read(byte reg)
{
  byte ret = 0;
  Wire.beginTransmission(L3GD20_ADDR);
  Wire.write(reg);
  Wire.endTransmission();
  Wire.requestFrom((unsigned int)L3GD20_ADDR, 1);
  while (Wire.available()) {
    ret = Wire.read();
  }
  return ret;
}


void setup() {
  Serial.begin(9600);
  while (!Serial) {}
  Wire.begin();
  Serial.println(L3GD20_read(L3GD20_WHOAMI), HEX);
  L3GD20_write(L3GD20_CTRL1, B00001111);

  MsTimer2::set(INTERVAL,clock);
  MsTimer2::start();
}

void loop() {
//三軸ジャイロセンサー
short X,Y,Z;
  float x, y, z;

  X = L3GD20_read(L3GD20_X_H);
  x = X = (X << 8) | L3GD20_read(L3GD20_X_L);
  Y = L3GD20_read(L3GD20_Y_H);
  y = Y = (Y << 8) | L3GD20_read(L3GD20_Y_L);
  Z = L3GD20_read(L3GD20_Z_H);
  z = Z = (Z << 8) | L3GD20_read(L3GD20_Z_L);


  anglex +=((double)((long)ANGLE_RATE_MAX*(long)INTERVAL)*((double)Backup_X+(double)X-2.0*Bias)/(double)INT_MAX/(double)1000/2.0)-0.0025;
  angley +=((double)((long)ANGLE_RATE_MAX*(long)INTERVAL)*((double)Backup_Y+(double)Y-2.0*Bias)/(double)INT_MAX/(double)1000/2.0)+0.0031;
  anglez +=((double)((long)ANGLE_RATE_MAX*(long)INTERVAL)*((double)Backup_Z+(double)Z-2.0*Bias)/(double)INT_MAX/(double)1000/2.0)+0.00395;
  count += 1;
  Backup_X =X;
  Backup_Y =Y;
  Backup_Z =Z;

  Serial.print(anglex);
  Serial.print("\t");  
  Serial.print(angley);
  Serial.print("\t");
  Serial.println(anglez);
  delay(10);

//測距レーザ
  int val;
  float distance;
  val =IDSread(0);
  distance = (AnaToCm(val));
  if (count % 100 == 0){
  Serial.print(" ");

  Serial.print(distance);
 Serial.print("m");
  Serial.print("\n");
  delay(100);
  }
}

//平均算出
int IDSread(int PinNo){
  long ans ;
  int i ;

  ans = 0;
  for (i = 0;i<1000;i++){
    ans = ans + analogRead(PinNo);
    }
    return ans /1000;
  }

//距離算出
float AnaToCm(float analogValue){
  float volt;
  float distance;
  volt = analogValue * 5.0 /1023;
  distance = (9.5974/volt)-3.0086;

  if(distance < 1.54) return(distance + (-4.79985/volt)+ 2.04432);
  if(distance >= 1.54)  return (distance + (-2.17484/volt)+0.781417);
  return(-1);
  }

補足情報(言語/FW/ツール等のバージョンなど)

言語はjava,測距レーザはSHARPのGP2Y0A710F,三軸ジャイロセンサーはSTマイクロL3GD20を使用しています。Arduinoのバージョンは1.8.3です。質問内容が稚拙でわかりづらい部分もあるかと思いますが、なにとぞご容赦ください。回答よろしくお願いします。