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

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

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



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

角度の計算

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

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

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

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

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

スケッチ

#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);

結果

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





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



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

スケッチ



#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);
}

結果

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



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

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

Tweet

スポンサードリンク