とりあえず通るつもりで作業を進めていきます。
一応跳びましたので、ここからは腕つけてバランスとるのがメイン作業となります。
設計は本分なので、まぁどうにかなるとして、テスト機を作った感じだと、制御がちょっと課題かな・・・と思ってます。
というのも、現状の制御周期が200Hzと遅く、制御がやりにくくなっています。
もともとのロボットの固有ダイナミクスも、小型化の影響で高周波よりになっているので、制御周期はもっと短くしたいところ。
とはいえ、200Hzより周期を上げると、タイマー割り込みのタスクが時間内に終わらずに、遅延が出る状況です。
なので、下記３つのどこかを短くする必要があります。
①単純な演算時間
②センサの計測値取得の際の、I2Cの通信時間
③他のプログラムにCPUを奪われている時間
①は、大した計算していないので、効かないんじゃないかと。せいぜいフィルタの行列演算に少し時間がかかってるくらい。
②は、加速度センサ、距離センサ、エンコーダ、ホールICと複数相手に通信しているので、結構かかっていそう。
でも通信速度上げるくらいしか手が無い。
③はOSを積んでいることの弊害といいますか。ココのサイトによれば、標準のイメージであるYoctoLinuxはAlmost real time OS らしいですが、
やっぱりRTOSでは無いので、他のプログラムに処理時間を奪われているのかも。
であれば、タスクの優先度の設定等で改善するかもしれません。
そこで③で改善できるのを期待しつつ、タスクの優先度の設定３種類で、処理時間を計測してプロットしてみました。縦軸は処理時間で、横軸は割り込みの発生回数です。プロットデータはフィルタ演算時間(Kfilt)と、センサ群との通信時間(I2C)の２つ。
他のプログラムにCPUを取られると、少なくともこの２つのどちらかで、時間がかかって見えるはずです。
１番上のグラフはタスクの優先度の設定無しの、デフォルトの場合です。タスクの処理時間が、回毎にばらついています。
２番目のグラフはMRAAでタスクの優先度をmaxに設定した場合。設定無しの場合に比べると、実行時間が僅かに短くなって、一定の処理時間に落ち着いているのが分かります。
３番目はRRポリシーでは無く、単純にFIFOで優先度を1に下げた場合です。２番目と大差ありませんので、上記サイトの記述通り、無暗に優先度を99まであげてもしょうがない、という事みたいです。
と言う事で、結論としては③は効いてはいるものの、②の方が改善の余地が大きいと言えそうです。
一応、EdisonのI2Cでは今の400KHzより早い3.4MHzが設定可能ではありますが、これがまた上手くいかない。
オシロが無いので憶測ですが、恐らく波形がなまってるのかな。これも調べるか、それとも手っ取り早くSPIに鞍替えするか・・・。
ちなみに、①のフィルタ処理も時間がかかっているように見えますが、こちらはリリースモードで走らせると劇的に短くなります。
使っている線形代数の演算ライブラリ(Eigen)の仕様なんでしょうかね？
***Edisonのタスク優先度設定のメモ***
・MRAAを使う場合
mraa::setPriority(99); //RR 0～99
・ポリシーなども設定する場合
#include <sched.h>
const struct sched_param priority={1}; // 0～99
sched_setscheduler(0,SCHED_FIFO,&priority);
