前回はマイコン側で、ホストPCからシリアル通信で受け取ったデータを意味のある形に変換・保持するプログラムを書きました。
おさらいですが、
- やり取りするデータはロボットの動作目標（目標傾き角、目標方向転換量）と制御ゲインの計５種類、全て２バイト。
- 負の値を取りうる最初の２つのデータについては、負の値の場合は通信の間だけオフセットをかけて正の値として扱う
でした。
今回はホストPC側の処理について簡単に説明していこうと思います。
前回から説明が説明の体を為していないというか、フワフワした話をゴチャゴチャと説明した挙句、githubの該当ページリンクを貼っただけのような気もしますが・・・
ホストPC側のプログラムの全体像
もう僕自身忘れかけていますが、軽くおさらいしますと、勉強も兼ねて、開発事例の多いROSを使ってホストPC側のプログラムを書こうと言う話でした。
マイコンができることは基本的に以下に限定していますので、それ以外の全てはホストPC側で計算します。
- 運動情報の取得 ： IMU情報、そこから求まる姿勢角、ホイールエンコーダ情報）
- モータの駆動 ： モータドライバへの指令値計算と送信
具体的には・・・それ以外の全てとしか言いようがありませんが、
- 外部入力（遠隔操縦とか）の情報処理
- 外界センシング関連
- ロボットの運動目標決定（止まるのか、動くならどう動くのか）
- 必要な情報のマイコンとの授受
あたりでしょうか。
今回はこの最後の部分を取り扱います。
I/Fノードの概要
ホストPC側はROSが動いていますので、
のが基本動作です。
ホストPCからマイコンへの情報送信
前回触れたように、マイコン側は、ヘッダバイト'H'と、５種類各２バイトの計11バイトを受信したらその情報を順番に処理していきます。
２バイトは上位１バイト、下位１バイトの順で送られてくることを前提にしていますので、確実にそうなるようにしていきます。
具体的には、送信したいある２バイトの情報に対して、右に8bitシフトさせることで上位１バイトの情報を、0b0000000011111111（0x00ff）とANDを取ることで下位１バイトの情報を、それぞれ取得します。
その後、ヘッダバイト、上位１バイト、下位１バイト、別の上位１バイト、別の下位１バイト・・・と情報をつなげていき、pyserialで送信するという流れです。
そもそも送る情報はというと、グローバル変数として定義しておき、別のノードからPublishされた情報をSubscribeして当該変数を更新することで取得します。
そのためグローバル変数が非常に多くなってしまっていますが、他に情報も思いつかないのと、このノード自体は２００行程度の小さなファイルですので、まあそれほどわかりにくくもなかろうということでそのままにしました。
マイコンからの受信
同じく前回説明したように、マイコンからは以下の１６種類の情報が送られてくるのでした。
- エンコーダのカウント（左右２種、各４バイト）
- 姿勢角（ロール、ピッチ、ヨーの３種、各４バイト）
- IMUデータ（加速度、角速度、地磁気各３軸+温度の計１０種、各４バイト）
- バッテリ電圧（１種、４バイト）
マイコンへの情報送信が終わったら、そのまま暫くマイコンからの返信を待ちます。
マイコンからの返信は合計64バイトのハズですので、受信バッファが64バイト溜まるまで待ちます（コード上は昔の名残（？）で60バイト溜まるまでしか待ちませんが、大差ないかと）
受信バッファに規定量のデータがあれば、受信完了とみなし、順番にデータを格納し、他のノードでも利用できるようにしていきます。
マイコンからホストPCへのシリアル通信は１行ずつ送られてきますので、１バイトずつ処理する必要が無くて楽です。順番に変数に格納してきます。
最初は改行コードを取り除いたり、少し慣れが必要でしたが・・・
ROSの標準メッセージ型でIMU情報がありますが、こちらは姿勢角をクォータニオンで取り扱いますので、それについては変換しておきます。
僕個人は知識不足でクォータニオンをどうこうする能力がありませんが、変換だけならできますので・・・
姿勢角とIMU関連情報はIMU型メッセージに格納してPublishします。
この情報は遠隔操縦やらその他ロボットの運動制御に使います。
エンコーダ情報はエンコーダ情報としてメッセージ型を新規に定義し、その型でPublishします。
この情報を基に、別ノードでホイールオトメトリを計算させます。
それくらい既存のパッケージがありそうですが、自分に扱えそうなものを見つけることができませんでした。
左右のエンコーダ情報からホイールオドメトリを計算するパッケージは、自作しました。それについても別で紹介したいと思います。
全体のコード
例によってコードはリンクを貼るにとどめます・・・ あまり参考にならないかもしれませんが。
whipbot_v2/mcu_interface.py at dev · KTD-prototype/whipbot_v2 · GitHub
まとめ
ゲインをいくつにするかは別問題ですが、適切に設定して、目標傾き角度、目標方向転換量をゼロに固定してしまえば、ロボットが安定して倒立しうる状態になりました。
次回はゲインを決める部分でも書こうかと思います。
ktd-prototype.hatenablog.com
