Inside PIDream

Training TracerのIMUを1軸だけ取得にすると1msでの制御周期ができそうなことは確認したが、実際に内部波形を見てみるともっと遅い周期で動いていそうなことに気づいた
エンコーダーとジャイロの値の更新が7ms毎に見えるので、検討してみる。

まずはI2Cの通信が影響していそうなので
cubeMXのファイルを開いて、I2Cの項目を確認する。
なんとI2Cがスタンダードモードの100kHzに設定されていた。
クロックをFast modeの400kHzに設定してFall timeとRise timeも0 nsにしてみる。

データの更新速度を確認すると、エンコーダーの更新速度は3msで更新されたが、I2Cジャイロは9ms毎に遅くなったように見える

次にsimulinkに戻ってジャイロの初期化関数を見てみる
レジスタ10hに07hが書き込まれている。BMX055のレジスタ10hはバントパスフィルタとサンプリングレート(ODR)のレジスタで、07hはODRが100Hz(フィルタ32Hz)の設定だった。

ということでODRを1000Hz設定の02hに書き換えてみる。

データの更新速度をモニターしてみると、
2ms毎更新まで詰めることができた

意図した制御周期で動いているかの確認方法が、監視と調整の時間と実時間の一致だけでは判断できないことが分かった。
さてどう確認すればいいものか？

Training Tracerのヨー回転運動の伝達関数を同定してコントローラーも設計してみる
①おもむろにsimulinkのブロック図を描く

直進にPWM duty50%を使い、回転の制御量にduty±40%を与える構成にしている。
今回は回転の入力にランダム信号を選択した。信号ソースはPN Sequence Generatorブロックを使用した
Bernoulli Binary Generatorブロックも試したが監視と調整で通信しようとするとXCP通信エラーがでて使いこなせなかった
PN Sequence Generatorブロックの出力を制御周期の5ms毎に切り替えると機体が全く応答しなかったので0.1s間隔で信号を切り替えるようにZero-Order Holdブロックを挿入している

②監視と調整で実機の応答波形を取得する

④MATLABのアプリからシステム同定アプリを起動して1次遅れ系と2次遅れ系の2種で伝達関数を推定した

1次遅れ系としての推定

2次遅れ系として推定

⑤シミュレーション用のsimulinkのブロック図を描く

モデルの部分はTransfer Fcnブロックではなく、LTI Systemブロックを選択してシステム同定アプリで導出したモデル名(tf1)を選択する

⑥コントローラーのゲインを調整する

今回もゲイン調整は自動ではうまく動かず。
１次遅れ系のモデルを使用すると、ちょっと無理な感じのハイゲインまで設定できたので、1次遅れ系は使用を見送り
（２次遅れ系のモデルでも結果的に実機よりもハイゲインまで動くようで、低いゲインから実機との相関をとっていく必要がありそう）

⑦シミュレーション環境でのコントローラーのゲインの合わせこみ結果

（実機と往復してゲインを下げていった最終のもの）

⑧実機制御用のsimulinkを描き、調整したコントローラーをコピペする

⑨監視と調整を行い、シミュレーションと実機の波形を比較する

青がシミュレーションの応答波形で、赤が実機の応答波形
何度かシミュレーションと実機を行き来した結果、ゲインが高すぎない場合はそこそこ一致できた

同定したDCモータの伝達関数を利用してコントローラを作成してみる
①おもむろにsimulinkでブロック図を作る

ステップ入力、Sum、PIDコントローラとTransfer Fcnブロックを配置してFB系を作る。Transfer Fcnには前回導出した伝達関数を入力する。
PIDコントローラは離散系にして、サンプリング時間は実機に合わせて0.005sに、実機に合わせて出力の上下限とアンチワインドアップも設定しておく。
②モデル設定のソルバーを固定ステップにして、時間を0.005sに設定する

③PIDコントローラーのゲインを調整する

シミュレーションの結果を見ながら調整を行う。
本当は自動調整を利用したかったが、離散系を選択するとゲインの自動調整ではゲインが強すぎて発振したため断念。

合わせこんだ結果の波形
④実機制御のslimulinkを立ち上げで、調整したPIDコントローラーをコピー貼り付けする

⑤監視と調整を行い実機の波形を取得する

緑がシミュレーション結果で、赤が実機の応答結果。かなり一致しているようだ

Training Tracerに付属しているエンコーダ付きDCモータの伝達関数(時定数)を推定してみる
①おもむろにsimulinkのブロック図を描く
DCモータへのステップ入力(0s-0.5sはPWM duty20%、0.5s-1.0sはPWM duty90%)と、制御周期(ステップ時間5ms)毎のエンコーダの速度を取得するブロックを作る。
そして二つの信号を後ほどシステム同定アプリで使いたいのでto Workspaceブロックに繋ぐ。信号の保存形式は２次元配列に設定する。システム同定アプリへの入力にdouble型が求められるので型の変換ブロックも追加する。
②simulinkの監視と調整で実機を動かす。実機が動いた後にこのような波形が取れる

③MATLABに戻るとworkspaceに、to Workspaceブロックで取得した配列が保存されている
④MATLABのアプリからシステム同定アプリを起動する

⑤システム同定アプリの右上側のimport dataからtimedomain dataを選択して
⑥出てきたimport dataの画面のinputとoutputにworkspaceの配列を入力、start timeは0に、Sample timeはsimulinkのステップ時間の0.005sを入力して、画面下のimportを押す
⑦画面中央のEstimateからtransfer Function modelを選択して
⑧Estimate transfer Function画面の極（pole）を1に零点（zero）を0に選択してEstimateを押す
⑨同定が完了する。この画面は閉じる
⑩システム同定アプリに戻るとft1の名前で結果が保存されているのでクリック。開いた画面で伝達関数を確認できる（時定数で37ms程度になる）

Embedded Coderの出力するコードの制御周期が本当にソルバーの固定ステップの指定時間周期で動いているのか疑問だったが、無理な値では正しく動かないらしい。そして、制御周期が間に合っているかの判断は、監視と調整の設定時間と実時間があっているかで判断できる。
ソルバーの時間を過度に短く設定すると監視と調整の終了時間を過ぎても終わらなくなり、例えば10秒設定なのに20秒間続くようになる。

Training Tracerの制御周期のボトルネックはジャイロセンサとのI2C通信のようで、Hardware Blockset の初期値ではジャイロのセンサのほとんど全ての値を取得しているために制御周期は8ms程度が限度になった。そこから加速度や磁気のブロックをコメントアウトすると4ms周期で動くようになり、Z軸のジャイロのみに絞ると1ms周期で動いた
Z軸ジャイロのみ:1ms
Z軸ジャイロ+Y軸ジャイロ:2ms
Z軸ジャイロ+Y軸ジャイロ+X軸ジャイロ:3ms
Z軸ジャイロ+Y軸ジャイロ+X軸ジャイロ+Z軸加速度:4ms
Z軸ジャイロ+Y軸ジャイロ+X軸ジャイロ+Z軸加速度+Y軸加速度:5ms

Training Tracer Ver.2の機体の仕様を知らずにHardware Blocksetを使って躓いたところ

→TrainingTracer_V3では0.1uFが追加されている

Training Tracer Ver.2のソフトをsimulinkで書く場合、Embedded Coder Support Package for STM32が必須になる。
Embedded Coder Support Package for STM32のセットアップ時には、さらに別のソフトのインストールを求められて戸惑った。
①ARM Compiler
コンパイラーを別途ダウンロードする必要があり、アカウントの作成も必要になった
②STM32CubeMX
STM32マイコンのプリフェラル設定そのコードを出力するソフト、ダウンロードにはSTMのアカウント登録が必要で、MATLAB上からそのソフトを動かすにはソフト上でのログインが必要