前回書いた様に倒立振子に挑戦してみる。
まず振子の姿勢を知るためにIMUを使用するので、M5AtomS3を制御に使用する。
姿勢を知るにはroll、pitch、yawを計算すれば良い。
M5AtomライブラリーではM5. MPU6886.getAhrsData()で得られるが、M5AtomS3ライブラリーにはない様だ。ただMahonyAHRS関連の関数はあるようなのでそれを使えば良さそう。
M5AtomS3/MahonyAHRS.h at main · m5stack/M5AtomS3 · GitHub
M5UnifiedにはMahonyAHRS関連は用意されてない様なのでMadgwickライブラリーを読み込んで使用する。
lib_deps =
M5Unified
arduino-libraries/Madgwick@^1.2.0
周期的に計算される事を前提としている様なので、最初タイマーを使用したがスタックを結構使用する様でスタックオーバーフローになってしまった。
タイマーに割り当てるスタックサイズを変更できることも書いてあったが、これからToioの制御もあるのでタスクを使用する事にした。(厳密には計算時間分遅れちゃうんだけど)
static void imu_task(void*)
{
int freq = 100;
madgwick.begin(freq);
while(true) {
float val[6];
M5.Imu.getGyro(&val[0], &val[1], &val[2]);
M5.Imu.getAccel(&val[3], &val[4], &val[5]);
madgwick.updateIMU(val[0], val[1], val[2], val[3], val[4], val[5]);
roll = madgwick.getRoll();
pitch = madgwick.getPitch();
yaw = madgwick.getYaw();
delay(1000 / freq);
Serial.printf("roll: %f, pitch: %f, yaw: %f\r\n", roll, pitch, yaw);
}
}
表示はM5Unifiedのサンプルを流用した。
M5Unified/HowToUse.ino at master · m5stack/M5Unified · GitHub
表示の判定にあるこの部分が最初理解できなかった。
if ((xpos[i] < 0) != (px < 0))
結果として左の条件と右の条件のXORを取っている事になっていた。
左は現在の値が負の値で、右は前回の値が負の値を判定している。
負の値 と 負の値 の場合は成立しない。
判定に直接出てこないが 正の値 と 正の値 の場合も成立しない。(0の場合も含まれる)
正と負 、または 負と正 と 符号が違う場合 に条件が成立する。
つまり前回と符号か変わったかを判定している。
この書き方は馴染みがなかったが、XOR判定するときに今度使ってみよう。
というか != はXOR演算子なのか。長い事ソフトをやっているが驚きの発見だ。
上からroll(赤)、pitch(緑)、yaw(青)を表していて、今回はrollを使えば良さそうだ。
