車輪の回転角はロータリエンコーダで計測します。エンコーダディスクには6コの穴が開いていて、モータの軸1回転当たり6パルスの信号をCPUに送ります。CD付属の倒立制御サンプルでは、この信号をプログラムで4逓倍し1回転当たり24パルスにしています。ここで、ロータリエンコーダ1パルス当たりの車輪の回転角を求める変換計数を算出してみます。
モータ1回転当たりのパルス数をPとします。
1パルス当たりモータは、 2*π/P [rad] だけ回転します。
ちなみに、[rad]はラジアンで2*π[rad] = 360[deg]です。普段の生活では「度」の方が広く使われているので1パルス当たりのモータの回転角は15°と言ったほうがなじみがありわかりやすいのですが、後々の代入操作などがラジアンのほうが便利なのでPUPPYのサンプルプログラムでの単位系ではラジアンを採用しています。
モータと車輪はギヤを介して接続しています。ギヤボックスのギヤ比をGとするとモータ1回転当たりの車輪の回転角は1/Gになり、1パルス当たりの車輪の回転角は
2 * π / P / G ＝ 2*π/24/29.8 ≒ 0.008785214 = K_C
車輪の回転角から車体の並進移動量はK_Cに車輪半径R_Wをかけて
K_C * R_W = 0.008785214 * 2.9cm ≒ 0.025477121 = R_C
となります。ロータリエンコーダのカウント当たりPUPPYはR_Ccmだけ進むことになります。
車体の重心回りの傾斜角速度はジャイロセンサで計測します。ジャイロセンサの出力は、
0.67 mV / deg / sec (村田製作所ホームページ中のPDFより）
です。センサの感受軸が1秒間に1°進む速さで回転しているとき、出力は静止時電圧（1,35V）+0.67mVになるということです。これをPUPPYで使いやすいようにするため、A/D変換値最小分解能 （5-0）/1024 当たりの回転角速度を求める変換計数を算出したいと思います。
まず、出力の単位系の mV/deg/sec を V/rad/sec に変換します。
1 [mV] = 0.001 [V] 1 [deg] = 2 * π / 360 [rad]
なので
0.67 [ mV / deg / sec ] -> 0.67 * 0.001 * 360 / 2 / π [V /rad/sec]
0.038388172 [V/rad/sec]
これに増幅回路のゲインをかけて
0.038388172 * （ 1 + 100/11 ）倍 = 0.387371553 [V/rad/sec]
となります。次にこの値の逆数の1V当たりの回転角速度[rad/sec/V]を求めます。
1 / 0.387371553 = 2.581500862 [rad/sec/V]
A/D変換値から電圧を求めるには、10ビットのA/D変換器なので
（5V-0V） / 1024
となり、これらを掛け合わせ
2.581500862 * 5 / 1024 = 0.012604984 = G_C
ジャイロセンサの取り付け方向と制御系の定義が逆になっているのでマイナスをかけると
G_C=-0.012604984
A/D変換値から静止時のA/D変換値を引いて、それにG_Cをかけると本体の傾斜加速度[rad/s]を求めることができます。
次に傾斜角です。 角速度は角度の変化の割合を表すもので、角度を時間微分すると算出することができます。逆に角速度を時間積分すると、角度の初期値さえわかっていれば角度を求めることができます。PUPPYのサンプルでは台形法を用いて積分を実現しています。
図1
マイコンのタイマを使って一定周期(サンプリング周期)を作ります。図1において角度は角速度と時間軸との間の面積になり、この面積を求めるには、サンプリング周期をなるべく短くし緑色であらわす台形を足し合わせていきます。ある時間ｋ+1の角度を求めるには角速度をωとすると
(ω(k)+ω(k+１))*T_T/2
これを制御開始時 ( k = 0 ) から足していくと現在の傾斜角が算出できるということになります。
