Arduinoで三軸ジャイロセンサーと測距レーザを両立させるにはどうすればいいか
解決済
回答 1
投稿
- 評価
- クリップ 1
- VIEW 1,540
前提・実現したいこと
現在、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です。質問内容が稚拙でわかりづらい部分もあるかと思いますが、なにとぞご容赦ください。回答よろしくお願いします。
-
気になる質問をクリップする
クリップした質問は、後からいつでもマイページで確認できます。
またクリップした質問に回答があった際、通知やメールを受け取ることができます。
クリップを取り消します
-
良い質問の評価を上げる
以下のような質問は評価を上げましょう
- 質問内容が明確
- 自分も答えを知りたい
- 質問者以外のユーザにも役立つ
評価が高い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。
質問の評価を上げたことを取り消します
-
評価を下げられる数の上限に達しました
評価を下げることができません
- 1日5回まで評価を下げられます
- 1日に1ユーザに対して2回まで評価を下げられます
質問の評価を下げる
teratailでは下記のような質問を「具体的に困っていることがない質問」、「サイトポリシーに違反する質問」と定義し、推奨していません。
- プログラミングに関係のない質問
- やってほしいことだけを記載した丸投げの質問
- 問題・課題が含まれていない質問
- 意図的に内容が抹消された質問
- 過去に投稿した質問と同じ内容の質問
- 広告と受け取られるような投稿
評価が下がると、TOPページの「アクティブ」「注目」タブのフィードに表示されにくくなります。
質問の評価を下げたことを取り消します
この機能は開放されていません
評価を下げる条件を満たしてません
質問の評価を下げる機能の利用条件
この機能を利用するためには、以下の事項を行う必要があります。
- 質問回答など一定の行動
-
メールアドレスの認証
メールアドレスの認証
-
質問評価に関するヘルプページの閲覧
質問評価に関するヘルプページの閲覧
checkベストアンサー
+1
角速度を1回測定する
積分して角度にする
レーザー測距を1000回行う
レーザー測距の平均を出す
とやっているようですが、これを
角速度を1回測定する
積分して角度にする
レーザー測距を1回行う
レーザー測距の過去1000回の平均を出す
とすれば良いです
投稿
-
回答の評価を上げる
以下のような回答は評価を上げましょう
- 正しい回答
- わかりやすい回答
- ためになる回答
評価が高い回答ほどページの上位に表示されます。
-
回答の評価を下げる
下記のような回答は推奨されていません。
- 間違っている回答
- 質問の回答になっていない投稿
- スパムや攻撃的な表現を用いた投稿
評価を下げる際はその理由を明確に伝え、適切な回答に修正してもらいましょう。
15分調べてもわからないことは、teratailで質問しよう!
- ただいまの回答率 88.23%
- 質問をまとめることで、思考を整理して素早く解決
- テンプレート機能で、簡単に質問をまとめられる
2017/11/08 11:04
回答ありがとうございます。その方法で試したら、うまく両方とも両立させることができました。
大変助かりました。