3ーさて前回まででカルマン・フィルタを使用したM5StickC傾斜計を作りました。M5StickCによる倒立振子を作っていきます。7月号を参考に倒立振子の実現を目指しています。月号トラ技の倒立振子はカルマンでの傾斜推測に加えてロータリエンコーダでタイヤの角度、角速度検出して倒立振子自体のモデルで更にカルマン・フィルタかまして安定度の高い倒立振子を実現しています。モータへの電圧供給フィードバックもなんか難しい関数で算出されています。今回はM5SickCを用いたカジュアルな倒立振子の実現を目指します。ロータリエンコーダは使わず、さらにモータへの供給電圧は難しい関数は使用せずに馴染みのあるPID制御で算出しました。動作まずは作ってみたとりあえず手を動かさないことには始まらないということで、倒立振子を作ってみた。傾斜角だけカルマン・フィルタかまして倒立振子してみた。チューニングのせいなのか、I2Cモータドライバの応答のせいなのか、出力が0.48Vからしかでないからなのか、もっと根本なのか。2019年6月24日やっぱ何も考えずに適当につくった倒立振子はどうあがいても全然倒立しませんでした。なじみ深いPID制御で供給電圧を算出しました。係数はBluetoothで送信しながら力業の手動チューニング。立った!立った!立ったー!やりぃー!キャッホー!!うれCY!!さていかにして、この倒立振子が実現されたかを以下に記します。構成I2CモータドライバDRV8830をM5StickCで制御してモータを駆動します。8個入りマシン以下が自慢のマシン詳細です。試して気づいたのですがマシン自体のバランスが非常に重要です(あたりまえか)。底面に鉄板貼り付けてバランス調整しています。($\dot{θ}$)をカルマン・フィルタをかまして推定値を算出して、その値を元にモータ供給電圧を決定して姿勢を床に対して垂直に保つようにします。ここではPID制御を用いて供給電圧を決定します。PはProportional(比例)、IはIntegral(積分)、DはDifferential(微分)です。(前回の誤差と今回の誤差の差)$$上式でモータ供給電圧を決定します。Kdを調整して、過度な供給や供給不足のない適切な電圧の供給を実現します。今回はBluetoothでM5StickCと通信してスマホで係数を送れるようして地道に手動で調整しました。SH200Qの設定値を調整しました。SH200Q用のクラスを修正して以下を変更しました。±250°/secまた出力レンジの変更に伴いヘッダファイルも修正しました。Kdとモータ供給電圧の最小値Vminをスマホから送れるようにします。スマホとM5StickCはスマホアプリのBlynkを用いてBluetooth通信させます。Blynkアプリのバージョンは2.27.6です。新規プロジェクトを作成します。TYPEはBluetoothを選択。TOKENはArduinoコード生成時に使用します(アカウント登録したメールに送信されます)。Inputウェジットを4つ配置します。Inputウェジットは±ボタンを押して値を増減させて送信できます。Inputウェジットの設定は最小値と最大値、ステップ数を指定してヴァーチャルピンに出力させます。以下のように設定しました。DRV8830DRV8830はI2C入力によって、モータ供給電圧(スピード)と供給電圧方向(回転方向)を制御するものです。正負の電圧を生成できるということです。A0のステートによって以下の表のようにアドレスを指定することができます。表のアドレス末尾xは読み込み時には1、書き込み時は0をしてします。ここでは書き込みのみ使用します。両方Openのアドレス:0x64としました。書き込みI2Cデータアドレス0x00に8bitの情報を書き込みます。各ビットの設定は以下の通り。上位6ビットで電圧を設定し、下位2ビットで正負を指定できます。IDEプログラム以下のBlynkのArduino用ライブラリを使用してプログラムしました。バージョンは0.6.1。https://github.com/blynkkk/blynk-libraryここではBluetoothを使用してM5StickCと通信します。参考にプログラムしました。カルマン・フィルタによる傾斜角と角速度の誤差の推定値を元にPID制御によってモータに電圧を供給して姿勢を保ちます。タイマ割り込み(2.5msec毎)でカルマン・フィルタ介して傾斜角と角速度の誤差を算出します。角速度に角速度の誤差を足して角速度の推定値としています。10msec毎に供給電圧を計算してモータ駆動します。初期設定時にセンサの測定値のオフセットを測って差し引くようにしてます。倒立振子によるPID制御のコードは以下を参考にさせていただきました。倒立振子を作ってみた。