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

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

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

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

java
1
2#include<MsTimer2.h>
3#include<Wire.h>
4
5//変数定義
6#define INTERVAL 10
7#define INT_MAX 32767
8#define ANGLE_RATE_MAX 500
9#define BUFF_SIZE 128
10
11const byte L3GD20_ADDR = B1101010;
12const byte L3GD20_WHOAMI = 0x0f;
13const byte L3GD20_CTRL1 = 0x20;
14const byte L3GD20_CTRL2 = 0x21;
15const byte L3GD20_CTRL3 = 0x22;
16const byte L3GD20_CTRL4 = 0x23;
17const byte L3GD20_CTRL5 = 0x24;
18const byte L3GD20_X_L = 0x28; const byte L3GD20_X_H = 0x29;
19const byte L3GD20_Y_L = 0x2A; const byte L3GD20_Y_H = 0x2B;
20const byte L3GD20_Z_L = 0x2C; const byte L3GD20_Z_H = 0x2D;
21
22int count;
23int GyroBuff[BUFF_SIZE];
24double Bias;
25int Backup_X =0,Backup_Y=0,Backup_Z= 0;
26double anglex =0.0,angley =0.0,anglez= 0.0;
27boolean gyoroGo = false;
28
29//タイマー割り込み
30void clock(void){
31  gyoroGo=true;
32  return;
33}
34
35void L3GD20_write(byte reg, byte val)
36{
37  Wire.beginTransmission(L3GD20_ADDR);
38  Wire.write(reg);
39  Wire.write(val);
40  Wire.endTransmission();
41}
42
43byte L3GD20_read(byte reg)
44{
45  byte ret = 0;
46  Wire.beginTransmission(L3GD20_ADDR);
47  Wire.write(reg);
48  Wire.endTransmission();
49  Wire.requestFrom((unsigned int)L3GD20_ADDR, 1);
50  while (Wire.available()) {
51    ret = Wire.read();
52  }
53  return ret;
54}
55
56
57void setup() {
58  Serial.begin(9600);
59  while (!Serial) {}
60  Wire.begin();
61  Serial.println(L3GD20_read(L3GD20_WHOAMI), HEX);
62  L3GD20_write(L3GD20_CTRL1, B00001111);
63
64  MsTimer2::set(INTERVAL,clock);
65  MsTimer2::start();
66}
67
68void loop() {
69//三軸ジャイロセンサー
70short X,Y,Z;
71  float x, y, z;
72 
73  X = L3GD20_read(L3GD20_X_H);
74  x = X = (X << 8) | L3GD20_read(L3GD20_X_L);
75  Y = L3GD20_read(L3GD20_Y_H);
76  y = Y = (Y << 8) | L3GD20_read(L3GD20_Y_L);
77  Z = L3GD20_read(L3GD20_Z_H);
78  z = Z = (Z << 8) | L3GD20_read(L3GD20_Z_L);
79
80
81  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;
82  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;
83  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;
84  count += 1;
85  Backup_X =X;
86  Backup_Y =Y;
87  Backup_Z =Z;
88
89  Serial.print(anglex);
90  Serial.print("\t");  
91  Serial.print(angley);
92  Serial.print("\t");
93  Serial.println(anglez);
94  delay(10);
95
96//測距レーザ
97  int val;
98  float distance;
99  val =IDSread(0);
100  distance = (AnaToCm(val));
101  if (count % 100 == 0){
102  Serial.print(" ");
103
104  Serial.print(distance);
105 Serial.print("m");
106  Serial.print("\n");
107  delay(100);
108  }
109}
110
111//平均算出
112int IDSread(int PinNo){
113  long ans ;
114  int i ;
115
116  ans = 0;
117  for (i = 0;i<1000;i++){
118    ans = ans + analogRead(PinNo);
119    }
120    return ans /1000;
121  }
122
123//距離算出
124float AnaToCm(float analogValue){
125  float volt;
126  float distance;
127  volt = analogValue * 5.0 /1023;
128  distance = (9.5974/volt)-3.0086;
129
130  if(distance < 1.54) return(distance + (-4.79985/volt)+ 2.04432);
131  if(distance >= 1.54)  return (distance + (-2.17484/volt)+0.781417);
132  return(-1);
133  }
134

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