記事一覧

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

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



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

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

MMA7361_15.jpg

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

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

構成

加速度センサーと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を使って計算する過程で影響が大きく出てしまうようです。

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

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

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

MMA7361_171.png

スケッチ

MMA7361_18.png

MMA7361_19.png

//差分方程式
#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);
}
}

動作確認



なんかイマイチです。なんとなく水平っぽくはなってきましたがスムーズではないし角度もずれてます。

研究を続けることにします。

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

スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

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

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

最新コメント

スポンサーリンク