arduinoを使った工作を幾つかやった後、倒立振子の作成を思い立ちました。今回の方針は次の2つです。

極力シンプルな構成
自力で作る（webの先人に頼らない）

立たせるのにまるまる1週間以上かかりましたが、なんとか当初の方針を守りながら、上の写真の倒立ロボットを作りました（下の動画は動作のデモです）。

（*1）制御の方程式とジャイロのトラブルシュートはwebを参照しました
（*2）動画では9V電池に代えてUSB出力のバッテリでarduinoに給電してます

この倒立ロボットですが、構造、電装、スケッチ（プログラム）ともにとても簡素です。使用するセンサは1つだけ（200～400円）で、モータは普通に手に入る一番安いヤツでOKです。なのでarduinoでLEDを点滅させたことがあれば、あとは材料とプログラムさえ揃えば、半日もかからずに作ることができると思います。

それで作成後の感想ですが、倒立振子の作成は、ちょっと難しいパズルを解くのに似ているように思います。答えが分かってしまえば簡単、というかパズルの意味がないので、そこに注意して話を進めます。

*An English version of this instructable is available.

** 日本語版では幾つかのコンテンツ（※）が表示されないようです。その場合、ブラウザのアドレスバーに表示されているURLの末尾「?lang=ja」を削除して画面を更新してください。それでも上手く表示されない場合は、英語版をご覧ください。
（※：各ステップの代表写真下の参考写真、PDFファイルのアイコン等）

*** ジャイロをSTマイクロ社製のデジタル出力センサに代えた新バージョン「もう一つの倒立振子（デジタル版）※」と、これらの倒立ロボットを用いた「倒立振子の研究」をアップしました（2014年9月）。それに伴い、本バージョン（オリジナル版）の最後にステップ12を追加し、これの記事で使うスケッチのリンクを添付しています。（※Step.2の微細なはんだ付けが不要です）

Step 1: 材料をそろえる

（1）躯体

　タミヤの「楽しい工作シリーズ」を使用します。

ユニバーサルプレートセット（2枚組、プッシュ・リベット付）
ユニバーサルアームセット（L字アングルが4個付属）
ダブルギヤボックス（左右独立4速タイプ、モータが2個付属）
スリムタイヤセット

（2）電装

arduino UNO
ブレッドボード（小）
ジャンパ・ケーブル（20本ほど）
モータ・ドライバIC（TOSHIBA TA7291P）を2個
ジャイロ・モジュール（秋月電子 K-04912）
単3電池を4本
9V電池（006P形）
電池ホルダ（単3×4本用）
電池スナップを2個
輪ゴム（数本）

材料紹介の動画（Step.1）

＜補足＞
ジャイロ・モジュールは個体のばらつきがあるようなので、2～3個買っておいた方が良いかもしれません。また最後に簡単に紹介しますが、モジュールを2つ使うと、倒立振子の動作が安定する気がします。

Step 2: 電装の準備

電装部品にピンとワイヤを取りつけます。

モータにリード線を取り付ける（はんだ付け）
ブレッドボード用のジャンパ・ケーブル（4本）を2つに切る
モータと電池スナップのリード線（全部で8本）に、この切断したケーブルをはんだ付けする
はんだ付けした箇所をテープで覆う（絶縁処理）
秋月電子製のジャイロ・モジュールにピンをはんだ付けする
このモジュールのコンデンサ（C6）の両端を細いワイヤでつなぐ（はんだ付け）

作業の解説動画（Step.2）

■解説（1）
使用するジャイロ・モジュールは、村田製作所のジャイロ・センサ（ENC-03R）を2つ実装しています。今回使うのはこの内1つだけです。このセンサは単位角速度（1deg/sec）あたり0.67mVを出力（Vo）し、モジュールのオペアンプ（NJM2155）でこれを10倍に増幅して1番ピンから出力します（6.7mV/dig/sec）。

問題は、このモジュールに付けられた2つのフィルタの高い方（HPF）で、これが角速度の変化率（角加速度）を混入させます。そこでこのフィルタを無効にするため、モジュールのコンデンサ（C6：4.7uF）の両端をショートさせて角加速度の混入を回避します。
（*1）参考サイト1
（*2）参考サイト2

Step 3: 躯体の組み立て

タミヤの「楽しい工作シリーズ」を使って車体部分を組み立てます。

ユニバーサルプレートにV字の切れ込みを作る（2か所）
ユニバーサルアームセットのL字アングル（4個）をユニバーサルプレートに取り付ける
ダブルギヤボックスを組み立てる（ギア比は114.7：1）
ダブルギヤボックスに付属モータ（2個）を取り付ける
ダブルギヤボックスをユニバーサルプレートに取り付ける
スリムタイヤセットの大径ホイールを組み立てる
スリムタイヤをダブルギアボックスに取り付ける

作業の解説動画（Step.3）

Step 4: 電装の組み立て

電装部品を組み立てて、躯体に取り付けます。

ブレッドボードにモータ・ドライバIC（2個）とジャイロ・モジュールを写真の通りに取り付ける
ブレッドボードに輪ゴムを巻き、躯体のL字アングル（下の方）に取り付ける
上の配線図を見ながらブレッドボードにジャンパ・ケーブルを刺していく
配線図を見ながらブレッドボードに電池スナップ（単3ホルダに接続する方）のピン（2本）を刺す
arduinoに輪ゴムを巻き、躯体のL字アングル（上の方）に取り付ける
配線図を見ながらarduinoにジャンパ・ケーブルを刺し、ブレッドボードと接続する
電池ホルダーに電池（4本）を入れる
躯体に輪ゴムを巻き、電池ホルダを取り付ける

作業の解説動画（Step.4）

■解説（2）
ここで作った倒立ロボットにはスイッチが付いていません。電源のON/OFFは電池スナップの接続／取り外しで行ってください。

Step 5: プログラムの書き込み

IDEを使ってarduinoにスケッチ（プログラム）を書き込みます。

IDEに「MsTimer2」が見当たらない場合（上の写真参照）、ダウンロードしてインストールする（*追記：タイマを使わない簡素版（ver.2.0）を追加しました（2014/06/06）。こちらを使用する場合、MsTimer2は不要です）　
arduinoとPCをUSBケーブルでつなぐ
電池ホルダに接続した電池スナップを外しておく
躯体を寝かせて静止状態にする
本ステップの最後にある「■サンプル・スケッチ（プログラム）」以下の内容を読む
サンプル・スケッチを収めたpdfファイル「invertedRobot.pdf」（もしくは「invertedRobot_v20_noTimer.pdf」）をPCに保存して（*）、その内容を全てコピーし、IDEでarduinoに書き込む　（* ブラウザで開いてコピーすると、スケッチの体裁が乱れて上手くいきません）

■解説（3）
倒立振子の制御は、モータの回転力の調整で行います。この調整は、振子の状態を表す4つの変数を使った簡単な数式で表されます。
モータの回転力＝　k1 × 躯体の傾き（角度）
　　　　　　　　　　＋k2 × 躯体の傾きの変化率（角速度）
　　　　　　　　　　＋k3 × 車輪軸の移動速度
　　　　　　　　　　＋k4 × 車輪軸の移動距離

ここでk1～k4は、これら4つの変数の重視の程度を表す定数です。末尾のpdfファイルのプログラムでは、72行目の長い式がこの調整式に相当します。倒立振子を立てるには、これら4つの変数を適切に評価するとともに、これらに掛る4つの係数の値を上手く決める必要があります。

ジャイロ・センサは、この式の2番目の変数（躯体の傾きの変化率）を計測し、これを電圧の高低で出力します。そして、この出力値をarduinoが積算することで1番目の変数（躯体の傾き）を評価します。

一方、3つ目の変数と4つ目の変数の評価は、モータもしくは車輪の回転速度を計測して行うのが標準的です。このため、ジャイロ・センサとは別のセンサ（回転計）か物理的なメータ（電流計やステッピング・モータ）を用います。

しかしここで作ったロボットは、ジャイロ・センサしか使っていないので、3番目と4番目の変数の評価が問題になります。末尾のpdfファイルのプログラムでは、76行目と77行目の式で、これら2つの変数の評価するのですが、とりあえずパズルとして空白にしています。

■解説（4）
もし、ここで紹介したジャイロ・モジュール以外のモジュールを使う場合、k1およびk2に相当する係数（プログラムの10行目と11行目）の値を変えてください。例えば、オペアンプ（NJM2155）をつけずにセンサ（ENC-03R）単独で使う場合、arduinoへの入力値は1/10になるので、係数k1とk2は10倍の値に変える必要があります。

■サンプル・スケッチ（プログラム）
まず下の「invertedRobot.pdf」もしくは「invertedRobot_v20_noTimer.pdf（*5）」をPCに保存し、これをadobe Reader等のアプリで開いてください。次にその中身（全部で3ページ）を全てコピーしてIDEに張り付け、下記の注記のようなミスプリを修正した上でarduinoに書き込んでください。

（*1）ファイルをブラウザで開いてコピーすると、スケッチの体裁が乱れます。必ずダウンロードしてからコピーしてください
（*2）72行目が切れている場合、次に置き換えてください
powerScale = ( kAngle * thetaI / 200 ) + ( kOmega * omegaI / 78 ) + ( kSpeed * vE5 / 1000 ) + ( kDistance * xE5 / 1000 ); //72
（*3）IDEやエディタにコピーすると、「//」の間にスペースが入る（「/ /」）ことがあるので修正してください
（*4）気温により動作が不安定になる可能性があります。夏期にバランスが上手くとれない場合、10～12行目の係数の値（45、85、57）を、それぞれ「52、95、53」あたりに変えてみてください（2014/06/06：追記）
（*5）「invertedRobot_v20_noTimer.pdf」はタイマ（MsTimer2）を使わない簡素版です。安定性も改善しているはずです（2014/06/06：追加）

Step 6: 動作の確認

振子（倒立ロボット）の動作を確認します。

プログラムの書き込みが終わったら、5秒ほど静止状態で放置する（写真①）
その後、躯体を起こして電池スナップを接続する（写真②）
車輪が回転を始めるので、両手でその回転を止める
車輪を床に接地させ、躯体重心が車輪軸上に乗るあたりで、躯体が動かないように静止させる（写真③）
ロボットが静止を確認すると、モーターの回転が緩み、「ミー」という小さなノイズがモータから出る
そっと手を離し（写真④）、ロボットの上端を指で軽く押さえる
この指を前後させ、ロボットが追従して動くことを確認する
押さえていた指を離すと、ロボットが少しバランスを取ろうとした後、転倒するのを確認する

作業の解説動画（Step.5～6）

■解説（5）
使用したジャイロ・センサは、躯体が動かない状態（角速度＝ゼロ）での出力値が決まっていません。このためarduinoの電源を入れた後（もしくはリセット・ボタンを押した後）、躯体を寝かせたまま5秒ほど放置してください。この間に静止状態での出力値を評価します。

■解説（6）
解説（5）の後、躯体を起こすと角度の変化を感知して車輪が回転し始めます。そこでロボットに、倒立時の標準的な姿勢（角度）を覚えさせる必要があります。このロボットは、躯体が0.05秒間静止していたら、その時の角度を標準とするようプログラムされています。

したがって、躯体を起こして車輪が回り始めたら、両手でロボットの下部を持って車輪の回転を強制的に止め、Step.6の4段目以降の操作を実行してください。

なお、しばらく静止させてもモーターの回転が緩まない場合、解説（5）の評価が上手くいっていない可能性があります。この場合、arduinoのリセット・ボタンを押して車輪の回転を止め、躯体を寝かせて静止させた後、再度リセット・ボタンを押してからStep.6を再実行してください。

■トラブルシューティング（1）
躯体を起こしても車輪が回転しない場合、もっとも疑われるのは配線の誤りです。Step.4に戻って慎重に確認して下さい。あと考えられるのはジャイロ・モジュールの不良です。私が最初に使ったモジュールはVccとGNDが短絡しており、配線をつなぐたびにarduinoとPCの接続が切れました。

また、車輪は回っても指の動きに追従しない場合、もっとも疑われるのは、モータのリード線もしくはジャイロ・モジュールの配置が逆になっているケースです。さらに、モータ・ドライバICとarduinoのデジタル・ピンの配線に誤りがあるケースもこれに準じます。Step.4に戻って慎重に確認してください。

Step 7: パズルを解く

転倒振子から倒立振子になるための壁をスケッチ（プログラム）の修正でクリアします。

ロボットが転倒する理由を考える
Step.5のプログラムに修正を加えて、転倒が防げるかどうか試す
試行錯誤が上手くいけば、転倒振子から倒立振子に脱皮して完成！（おめでとうございます）
完成したらStep.8を覗いた後、Step.9のロボット操作を試してみる
試行錯誤が上手くいかなかったら、「1.」に戻るかStep.8（解答案）を見るかどうか悩む

Step 8: パズルの解答案

プログラムを修正して、振子を倒立させます。

Step.5のプログラムの76行目と77行目を次の3行に書き換える（上書きする）

vE5 = sumPower; //76a
xE5 = sumSumP / 1000; //77a
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

Step.6を実行する（8行目以降の指による支持は不要）
手で静止させた状態で躯体の重心が車輪の軸上に近いところにあれば、ロボット（振子）は前後に少し往復した後で倒立状態に移行する
振子がバランスをとりながら倒れないことを確認する
振子がバランスを取りながら、少しずつ移動していく場合、74行目の「power」の後に、適当な数値（1～5程度の整数）を足し引きしてみる

作業の解説動画（Step.8）

■トラブルシューティング（2）
Step.6をクリアしたにもかかわらず、振子が倒立せずに倒れてしまう場合、まず電池（単3×4本）を新しいものに換えてみてください。それで解決しない場合、Step.5のプログラムの10行目から13行目の数値を変えてみてください。この内、最も効果があるのはたぶん12行目の数値だと思います。

Step 9: シリアル・モニタを使った振子の操縦

シリアル・モニタを使って、振子の向きと前進／後退の操縦を行います。

Step.8で修正したプログラムに後述の修正（4か所）を追加する
追加修正したプログラムをIDEでarduinoに書き込む
書き込みが終了したら、IDEからシリアル・モニタを起動する
シリアル・モニタの右下を確認し「LFのみ」「115200 baud」に変更する
5秒ほど振子を放置する
Step.6を実行して振子を倒立させる（8行目以降の指による支持は不要）
シリアル・モニタの上部の入力窓に、0〜3の数字を入力して送信ボタン（もしくはEnterキー）を押す
振子の姿勢や動作が変わることを確認する

作業の解説動画（Step.9）

■解説（7）
シリアルモニタを使って、振子を回転させたり前後に移動させたりします。例えば、「00」と入力すると、「0」を2回入力したことになり回転幅が大きくなります。
・右回転･･･「0」を入力
・左回転･･･「1」を入力
・前進･･･「2」を連続して入力すると前方向に加速します
・後退･･･「3」を連続して入力すると後方向に加速します

■スケッチ（プログラム）の追加修正（4か所）

（1）Step.8で修正したプログラムの17行目と18行目（*）の間に、次の5行を追加（コピー）する　（*行番号は、Step.5のpdfファイルで各行右側に付記している行番号（//xx）に準じます）

volatile int drct = 0;
volatile boolean right = false;
volatile boolean left = false;
volatile int fwdBck = 0;
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

（2）さらに元のプラグラムの32行目を削除し、代わりに次の26行を追加（コピー）する

if ( Serial.available() ) {
drct = Serial.read();
Serial.println(drct);
}
if( drct == 48 ) { right = true; }
else if ( drct == 49 ) { left = true; }
else if (drct == 50 ) { fwdBck++; drct = 0; }
else if (drct == 51 ) { fwdBck--; drct = 0; }
if ( right == true ) {
analogWrite( 6, 140 );
digitalWrite( 4, HIGH );
digitalWrite( 5, LOW );
analogWrite( 9, 140 );
digitalWrite( 7, LOW );
digitalWrite( 8, HIGH );
delay(40);
} else if ( left == true ) {
analogWrite( 6, 140 );
digitalWrite( 4, LOW );
digitalWrite( 5, HIGH );
analogWrite( 9, 140 );
digitalWrite( 7, HIGH );
digitalWrite( 8, LOW );
delay(40);
} else if ( power > 0 ) {
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

（3）さらに元のプラグラムの55行目と56行目の間に、次の4行を追加（コピー）する

drct = 0;
right = false;
left = false;
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

（4）最後に元のプラグラムの74行目を削除し、代わりに次の2行を追加（コピー）する

sumPower = sumPower + power + fwdBck * 4; // 74a
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

Step 10: USBケーブルの切り離し

arduinoの電源を電池に代えて、倒立振子を完成させます。

arduinoからUSBケーブルを抜く
2つ目の電池スナップに9V電池を接続する
この電池をロボットの背部に、電池ホルダと共に輪ゴムで取り付ける
この電池スナップのプラス側の線をarduinoのVinピンに刺す
この電池スナップのマイナス側の線をarduinoのGNDピンに刺す
電池ホルダー（単3×4本）に１つ目の電池スナップが接続されていれば、車輪が回転し始める
Step.6を実行して振子を倒立させる（完全な自立）
完成

作業の解説動画（Step.10）

Step 11: 発展

完成したロボット（倒立振子）を改良してみましょう。（二つ目の改良まで半日で行うのは、さすがに難しいと思います）

（1）ジャイロ・モジュールを追加

ジャイロ・モジュールをもう一つ追加して、倒立の姿勢や操縦の安定が改善するか確認します。

確認動画（Step.11-1）

（2）無線で倒立ロボットを遠隔操作

部品点数を増やさないようにテレビの赤外線リモコンを使います。

操縦動画（Step.11-2）

（3）Excelでシミュレーション

振子を車輪と胴体と頭の3つに簡略化して考えると、高校物理の範囲で運動方程式が書けます。これもパズル感覚ですが、運動方程式が書ければ、Excelでシミュレータを作れます。Step.5の2つの係数（k1とk2）の大きさは、これで概略あたりを付けることができます。

Step 12: 「もう一つの倒立振子（デジタル版）」と「倒立振子の研究」の資料

使用するジャイロ・モジュールとモータ・ドライバーICを変更した別バージョン（改良版）を、別のインストラクタブルとして追加しました（2014年9月）。そのステップ5で用いる2つのスケッチを収めたpdfファイルを添付します。

invertedRobot_v20d_noTimer.pdf
digtlGYRO_L3GD20_SPI_recover.pdf

さらに同月、これらの倒立ロボットの動作を分析し、PC上で「仮想振子」を描く方法を、「倒立振子の研究」のタイトルで別途投稿しました。そのステップ3とステップ9で用いる2つのケッチを収めたpdfファイルも併せて添付します。

virtualPendulum_processing.pdf　･･･　Processingのスケッチ
invertedRobot_v21d_crrnt_accl.pdf　･･･　Arduinoのスケッチ

なお、これらpdfファイルのアイコンが表示されない場合、アドレス・バーのURLの末尾の「?lang=ja」を削除して、ページを更新してください。

Introduction: 半日で作る倒立振子