一時的に話が逸れていましたが、倒立振子ロボットver2の話の続きを書いていこうと思います。
前回までで、ESP32系マイコンを使ってエンコーダ付DCモータの制御ができるようになったところでした。
番外編ではIMUとのインターフェース部分について、Arduinoで使っていたコードの再利用を試みたところですが、どういうわけかI2C通信が安定せず、SPI通信を使って取得することにしました。
今回は、それらの情報を統合してモータのPID制御を行い、それから機体の情報をホストPCに送ったり、ホストPCからPIDゲイン情報を受け取ったりする部分を書いていきます。
ホストPCからの情報受信
今まではコマンドサーボ（シリアルサーボ）を使っていたので、USBシリアル通信の部分はホストPCとサーボのシリアルインターフェース間で行われていました。
実際のコードはpyserialを使ってチマチマとコマンド（制御対象サーボのID、制御モード、制御量とか）を送って、返信を受け取って、解読して、を繰り返す感じ。
今回も基本は一緒で、必要な情報はシリアル通信でやり取りを行いますが、コマンド体系が決まっていませんので、自分でなんとなく通信のルール（？）を決めなければなりません。ルールというと大げさですが・・・何の情報を何バイトで受け取って、どのように処理するか、という話です。
特に、Arduino言語はシリアル通信で受信した情報を１バイトずつ読み取っていくようなので、２バイト以上の数字を受け取る場合は、１バイトずつ分割して受け取った後、統合して意味のある数字を再構成するための工夫が必要です。
全て１バイトの情報なら良いのですが・・・ちょっと考えた結果、１バイトだと情報量に制限が強く、ゲイン情報等の細かい設定ができなくなるのではと考え、一部は１種類の情報当たり２バイトとしました。
通信については、下記のサイトを参考にさせてもらいました。
kougaku-navi.hatenablog.com
上記のサイトを真似て、ホストPCのpython（ROSノードの一部）から、ヘッダ'H'を含めたデータを送信し、１バイトずつ受信していきます。
受信するデータは以下の５種類、全て２バイトです。ヘッダバイトを含めて合計１１バイトですね。
- 目標傾き角度 ： 倒立振子ロボットの目標傾き角度。基本は０°ですが、重心に偏りがあれば若干調整しますし、前進・後退させる場合はその方向に傾けることになります。データは通信時のみミリラジアン（整数でやりとりするため）で扱い、受信後は1000で除してラジアンとして扱います。
- 目標方向転換量 ： 倒立振子ロボットが向きを変えるため、左右モータに送るPWM信号に正負反転したオフセットを与えます。無次元です。
- 傾き角度制御の比例ゲイン ： そのままの意味です。
- 同、積分ゲイン
- 同、微分ゲイン
注意すべきは、最初の２種類の値は負の値を取りうるということ。
残念ですが、負の値の処理をどうすれば良いのか自信がありません。
幸い、近藤科学のサーボモータでは、負の値を取り扱う場合は、「当該数に既知の大きな数を足す」ことで正の値に変換して使っていました。２バイトであれば65536通りの数字が使えますので、この値ですね。
正の値（0〜32000）を取り扱う場合はそのまま、負の値（-32000〜-1）を取り扱う場合はそれに65536を足して、正の値（33536〜65535）に変換します。
今回もそれを使えば良いのですが、コードを書いていた当時は、キリの悪い数字を嫌ったのか忘れましたが、それほど大きい値は使わないだろうということで、基準の数を32000として扱いました。
これで理論上、正（非負）の値は0〜16000、負の値は-15999〜-1を16001〜31999として、トータルで-15999〜16000の値を扱えるようになりました。
PIDの各ゲインは正の値しかとりませんので、そのまま使います。
マイコンからホストPCへの情報送信
ESP３２もそれなりの計算能力がありますので、逐一情報をホストPCに送信する必要は無いのですが、いずれ自律移動をさせるときとか、ホストPCも同じ情報を持っていた方が便利だろうと思うことも多いので、それなりの量の情報は送信するように作っていきます。
上記に書いたようなホストPCから送られてきた制御目標、ゲイン等の情報の受信が全て完了したら、そのままその時のロボットの状態量について、最新の情報をホストPCに送り返します。
送り返す情報は以下です。かなり多いですが・・・ バッテリ情報は正直ホストPCに送る必要があるかどうかは疑問です。
それ以外の情報は、一応ホストPCが持っていると、SLAMとかやるときに良いことがあるのではないかと思っています。
- エンコーダのカウント（左右２種、各４バイト）
- 姿勢角（ロール、ピッチ、ヨーの３種、各４バイト）
- IMUデータ（加速度、角速度、地磁気各３軸の計９種、各４バイト）
- バッテリ電圧（１種、４バイト）
マイコンによるモータ制御
基本的なモータの回し方、エンコーダ情報の取得・回転量演算は前回説明したとおりです。
新たに加わったのは、モータの制御目標の部分ですね。
この部分は、よく倒立振子の姿勢制御で使われているPID制御を使っています。
ゲインは前述のとおりホストPCから送られてきたものを格納します。
時々刻々変化させる可変ゲイン式の制御を行う訳ではないですが、安定稼働までの試行錯誤でも頻繁に更新しますし、実験条件や車体に搭載したセンサの重量、配置によって頻繁に更新することが考えられたので、通信容量は食いますが、常にホストPCから姿勢角目標等と一緒に送信することにしています。
PID制御による制御対象は、車体の姿勢角です。直立を0°としており、それが制御目標（基準値）となりますが、重心位置によってこちらも頻繁に変わりますし、車体を任意に移動させるようになれば時々刻々更新する値になりえますので、こちらも前述のとおりホストPCから逐次送信しています。
姿勢角の現在値、目標値、その微分値・積分値をマイコンで計算してPID制御を行います。制御出力は、モータドライバに送るPWM値となります。
マイコン上で動かす実際のコード
乱暴ですが、以下を見るのが手っ取り早いです。
github.com
コードの流れをもう一度整理しますと、以下の流れになります。
- ホストPCから送られてくる制御ゲインと目標姿勢角、IMUにより計測した現在姿勢角に応じた、PIDによるモータ制御
- （タイマー割込みにより）IMUの情報取得、姿勢角の更新
- （ホストPCから送信された情報の受信が完了したら）制御ゲイン、目標姿勢角の更新
自分なりに無駄は削ったつもりなのですが、あまりに長いコードなので、今回はコードの転載はやめておこうと思います・・・
細かいですが、モータに入力するPWM値が極端に変化しないよう、１サイクルで変化する最大量に制限をかけたり、バッテリ電圧（モータドライバへの入力電圧）が下がってきたらLEDを点滅させたりとちょっとした小技を挿入しています。
次回はホストPC側の通信インターフェースの解説でもしたいと思います。
