記事一覧

3軸加速度センサモジュール MMA7361LC その③Arduinoで角度検出

3軸加速度センサモジュール MMA7361LC その③Arduinoで角度検出



こんにちはRoboTakaoです。ようこそ「極力ローコスト ロボット製作 ブログ」にお越しくださいました。

昨日は加速度センサーMMA7361LCをArduinoに接続して加速度を測りました。今回は加速度から角度を計算してみます。
ついでにその角度でサーボを動かしてみます。

MMA7361_14.png

スペック等は以前の記事を参照してください。

角度の計算

X軸とY軸周りの角度は以下の式で表すことができます。

X軸: Ax = arctan ( Yg / Zg ) YgはY方向の加速度、ZgはZ方向の加速度

Y軸: Ay = arctan ( Xg / Zg ) XgはX方向の加速度

Arduinoの場合arctanはatan( )という関数でmath.hをインクルードして使います。

単位はradなので180°/πを掛けます。

スケッチ

MMA7361_08.png

#include
float val0,val1,val2;;
float XX,YY,ZZ;

void setup() {
Serial.begin(9600);
}

void loop() {
val0 = analogRead(0);
val1 = analogRead(1);
val2 = analogRead(2);

XX = -(val0/1024*5000-1650-74.6)/800;
YY = -(val1/1024*5000-1650-260.7)/800;
ZZ = -(val2/1024*5000-1650+141.7)/800;

Serial.print("X:");
Serial.print(XX);
Serial.print("G Y:");
Serial.print(YY);
Serial.print("G Z:");
Serial.print(ZZ);
Serial.print("G X_axis:");
Serial.print(atan(YY/ZZ)/3.142*180);
Serial.print("deg Y_axis:");
Serial.println(atan(XX/ZZ)/3.142*180);
delay(300);
}

結果

シリアルモニターで確認します。

MMA7361_09.jpg

MMA7361_10.png

うまく角度データを読み込めています。



ついでにサーボも動かしてみましょう。

スケッチ

MMA7361_12.png

#include
#include

Servo myservo2;
float val0,val1,val2;;
float XX,YY,ZZ;
int ang = 90;

void setup() {
myservo2.attach(2);
Serial.begin(9600);
}

void loop() {
val0 = analogRead(0);
val1 = analogRead(1);
val2 = analogRead(2);

XX = -(val0/1024*5000-1650-74.6)/800;
YY = -(val1/1024*5000-1650-260.7)/800;
ZZ = -(val2/1024*5000-1650+141.7)/800;

ang = 90+atan(YY/ZZ)/3.142*180;

myservo2.write(ang);

Serial.print("X:");
Serial.print(XX);
Serial.print("G Y:");
Serial.print(YY);
Serial.print("G Z:");
Serial.print(ZZ);
Serial.print("G X_axis:");
Serial.print(atan(YY/ZZ)/3.142*180);
Serial.print("deg Y_axis:");
Serial.print(atan(XX/ZZ)/3.142*180);
Serial.print("deg ang:");
Serial.print(ang);
Serial.println("deg");
delay(300);
}

結果

ブレッドボードを回すとその角度に応じてサーボを動かすことができました。



遅れがあるのはサーボのスピードが遅いせいです。

それでは今回はこの辺で失礼します!ありがとうございました。

スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

Author:RoboTakao
みなさんご訪問ありがとうございます。ロボット作りたいけどお小遣いそんなにないし、簡単でローコストでロボットを作るための私のプロジェクトを紹介します。

ウェブサイトもありますのでそちらもよろしくお願いします。
http://robotakao.jp/

最新コメント

スポンサーリンク