###前提・実現したいこと
現在、Arduinoで、三軸ジャイロセンサーと測距レーザの二つを利用する装置を作成しています。具体的には、装置の高さをあらかじめ決定し、三軸ジャイロセンサーで測定した角度を利用し、三角関数で距離を求めます。そして測距レーザで実際求めた距離とどれ位差があるかを求めるというものです。
###試したこと・問題
ひとつのプログラミングにまとめたのですが、三軸ジャイロセンサーは瞬間加速度で角度を求めるプログラミングで、測距レーザは平均を出して距離を求めるプログラミングとなっているため、相性が非常に悪く、角度を正確に出そうとすると、レーザで算出した値を平均する総数を減らさないといけず、距離が正確に測れなくなってしまいました。そして、距離を正確に出そうとすると、平均の総数を増やさなくてはならず、そうすると、処理がおちてしまい、角速度を正確に測ることができず、角度を求めることができなくなってしまいました。この二つを両立させるにはどうすればいいでしょうか。
また、両立させることは不可能でしょうか。
###該当のソースコード
ひとつにまとめたプログラミングを記述します。短くする方法がわからないのでこのままで失礼します。
java
#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です。質問内容が稚拙でわかりづらい部分もあるかと思いますが、なにとぞご容赦ください。回答よろしくお願いします。
まだ回答がついていません
会員登録して回答してみよう