３軸加速度センサモジュール MMA7361LC　その④Arduinoで角度検出して水平をキープする（途中）

2017/10/22
23:56

３軸加速度センサモジュール MMA7361LC　その④Arduinoで角度検出して水平をキープする（途中）

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

前回までに加速度センサーMMA7361LCをArduinoに接続して加速度を測って、角度を計算してみました。
今回は２個のサーボを使って水平をキープする機構を作ってみたいと思います。
ですが、今のうちに書いておくとうまくいっていません。途中経過となります。

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

前回までの記事はこちらえお参照してください。

構成

加速度センサーとArduino Nanoをブレッドボードに載せて、

そのブレッドボードにホットボンドでサーボをくっつけました。

デジタルフィルター

加速度データをそのまま使うとノイズやら誤差やらで角度は発散してしまい、水平をキープする制御では使えません。

そこで今回は差分方程式方式のデジタルフォルターを入れたいと思います。と言っても初めての使用したのでググりました。

これは回路のRCフィルタと同じ機能ということらしいです。

基本的な考え方

[今回のデータ] = a x [前回のデータ] + (1-a)x[今回計測したデータ]

a = 0.8とか0.9くらいを入れるようです。
式を見てわかるように a は重み付けを行っていて、8〜9割は前回のデータを使い、1〜2割を新しい計測値を使う式です。

フィルターを使うとノイズを除去できますが反応が鈍ります。今回のような応答性も重要な機構においてはこの a を調整することになります。aを小さい値にすると応答性が良くなりますが、ノイズが除去しにくくなります。

とりあえずa = 0.8でやってみました。

角度計算見直し

前回までで加速度とarctanを使って角度計算していましたが、どうもずれてしまいます。

やはりノイズや誤差があると計算の上ではarctanを使って計算する過程で影響が大きく出てしまうようです。

いろいろ係数をかけたり、オフセットさせたり試みたのですがうまくいきません。

そこで思い切って直接　角度と検出電圧でプロットして近似式を作ることにしました。

さらに本来ならば３次式くらいで計算したいのですが、Arduinoでは厳しそうなので-60〜60度に限定して線形近似することにしました。

スケッチ


//差分方程式
#include 
#include  

Servo myservo2;
Servo myservo3;

float val0,val1;
float val00 = 0;
float val01 = 0;

float ang20,ang21,ang30,ang31;
int ts=50;m
int td=5;
int a2,b2,a3,b3;
float c=0.8;

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

void loop() {

  val0 = analogRead(0);
  val1 = analogRead(1);

  val00 = c*val00 + (1-c)*val0;
  val01 = c*val01 + (1-c)*val1;
 
  ang20 = ang21;
  ang21 = 90+(0.0757*(val01/1024*5000)-143.03);
  ang30 = ang31;
  ang31 = 90-(0.0784*(val00/1024*5000)-135.7);

  Serial.print(val00/1024*5000);
  Serial.print(" ");
  Serial.print(val01/1024*5000);
  Serial.print(" ang2:");
  Serial.print(ang21);
  Serial.print("deg ang3:");
  Serial.print(ang31);
  Serial.println("deg");

  a2 = ang21 - ang20;
  b2 = ang20;
  a3 = ang31 - ang30;
  b3 = ang30;

  for (int k=0; k <=td-1 ; k++){
     myservo2.write(a2*k/td+b2);
     myservo3.write(a3*k/td+b3);
     delay(ts/td);
  }
}