角速度制御失敗。

今日は、角速度制御の出だしを書きました。
いえ、書いたつもりでした。

今回のマウスのスラロームは、予め計算してある1/rテーブルに速度をかけ、角速度を算出し、それに対して制御をかける事によって、速度に依らず一定の軌跡を描く事ができるような構成です。
1/rテーブルは、Windows上のプログラムで計算しています。

適当に書いてみた結果、うまく行きませんでした。
速度が速くなればなるほど、旋回角度が深くなってしまいます。

結局、部室を出るまでに解決する事はできなかったのですが、帰りの電車を待つ途中、ふと思い出しました。

目標角速度に対して左右のタイヤの速度を変える時、
(speed_r * (1+ang_v))
(speed_l * (1-ang_v))
みたいな感じで書いてました。
(speed_r + (T/2)*ang_v)
(speed_l - (T/2)*ang_v)　//Tはタイヤ間の間隔
が正解です。

前者の式だと、左右のモータを、ある回転半径に追従させる　みたいな感じになってしまっています。
後者が、角速度に追従させようとしている式でした。

帰りの電車の中で、改めて絵を色々書きながら考えて納得しておきました。よく理解しないままプログラムすると痛い目を見る世界のようです。

あと、その過程でラジアンの使いやすさを改めて実感しました。
今は制御をdegベースで書いていますが、radに直そうかな。

ついでに角運動するときのFFの式をでっち上げてみた所、簡単そうだったので、余裕があれば明日実装してみようと思います。
慣性モーメントを一発で求めてくれる計測機が欲しいです。

※追記
慣性モーメント、マウスで計測すればいいですね。