おびやまロボット研究所　～マイコン電子工作と自作ドローン～

Arduinoで倒立振子

Arduinoの使い方を学ぶために何かやってみよう，と思って集めた部品がこれ↓



デジタル出力端子４本から信号を受けてMOSFETモジュールを通してステッピングモータを駆動．アナログ端子にジャイロを接続しました．



ステッピングモータを回すのsketchのExample（Library-Stepper）があったので，モータは簡単に回りました．ジャイロを動かすと，動かす方向に応じてモータが回ります．

モータを並列に２個配線して，



あっという間に倒立振子のできあがり．早速プログラムを作って書き込んでみます．



立った！！　



終わり．


．．．本当は，立てるのは結構大変です．
倒立振子を作るのは１時間くらいでしたが，立てるには数時間かかりました．
単純に，ジャイロの信号で倒れない方向にモータを回すだけでは立ちません．
まず，タイマー割り込みを使ってジャイロの信号から角度を得て，次にジャイロの信号を積算して角度信号に直します．

参考URL
http://www.arduino.cc/en/Reference/Libraries

角度が小さいうちは，角度に比例した移動速度と移動距離を与えるとパラメータが上手く絞り込めれば立ちます．でもこれではある程度以上の傾きになるとパタンと倒れます．
角度が大きくなると倒れるスピードが速くなるので，補正も早くかけないといけません．
ただし，ジャイロのドリフトがあるのでほっとくと倒れます．
また，補正スピードが上がるとモータが脱調してしまってパラメータの絞込みが大変でした．
おまけに，途中で電源を逆挿しして，レギュレータが音を立てて燃え，レギュレータを交換したら生き返って一安心．

次は，ドリフトの補正，位置決め，左右モータの独立制御をやってみる予定．

買うなら↓


倒立振子の仕組みが学べますヴイストン Beauto Balancer Duo


--追記--
最初に作った一番シンプルなsketchを追記します．
まず，setup中でジャイロのオフセット値を読み取り，
5msのタイマーでgyro()関数を起動し，ジャイロの値を読み取って積算して角度（angle）とします．
loop()の中でステップスピードを角度変位に比例して変化させ，
stepも角度変位に比例して設定します．
ほぼ垂直にしてリセットをかけるとその時の角度を維持しようと動きます．
比例係数を適当に調整すれば，角度変位が小さく，ジャイロのドリフトが小さく，短い時間なら立ちます．

/* 倒立振子 */

#include < Stepper.h >
#include < MsTimer2.h >
#define STEPS 100

Stepper stepper(STEPS, 8, 9, 10, 11);
int goff=0;
long angle=0;

void gyro() {
angle=angle+(analogRead(0)-goff);
}

void setup()
{
if (goff==0) goff = analogRead(0);
MsTimer2::set(5, gyro); // 5ms period
MsTimer2::start();
}

void loop()
{
stepper.setSpeed(80+abs(angle)*1.5);
stepper.step(angle*0.1);
}