全60Stepで超超簡単M5StickC 倒立振子

#■はじめに
###以前書いた下記の記事のコードをさらに簡略化し、60Stepまで大幅短縮したものです。車体は全く同じなので下記の記事を参考にして下さい。
###実質500円&100Stepで作る超簡単「 ゆるメカトロ的 M5StickC 倒立振子」

#■以前の記事からの主な変更点
###①姿勢推定ロジックを「getAhrsData」関数で置き換え
以前の記事では加速度とジャイロから姿勢推定計算を行っていましたが、これをM5StickCが提供している下記の関数で置き換えることで一発で姿勢の取得ができ、大幅にコードを簡略化できました。この関数は内部で、「カルマンフィルタ」よりも精度がよいと噂されている「Madgwickフィルタ」を呼んでいるようです。
###M5.MPU6886.getAhrsData(&pitch,&roll,&yaw);

車体へのM5StickCの組付け形態からすると、車体の前後運動はpitchに反応があるのかなと思ったのですが、実際はrollに反応があったのでそれを使いました。理由は不明です。
###②キャリブレ操作不要になり、いきなり立たせられます
#■倒立動画

#M5StickC #倒立振子
全60Stepの超超簡単倒立振子完成。
MPU6886による姿勢推定を「Accl/Gyroによる計算」→「Roll角ダイレクト取得」に変更で大幅に簡略化。Qiita投稿準備中。 pic.twitter.com/XXWNflC7CH

— Google Homer (@google_homer_) February 29, 2020

バッテリーの持ちの短さが悩み。 #■コード **たったの60行にまで簡略化できました。** **立派に立たせるには、あいかわらず地味にコツコツPID係数を調整する必要があります。***

#include <M5StickC.h>
//PID係数
#define TARGET            -83.4
#define KP                100.0
#define KI                2.0
#define KD                2.0
//Motor
#define MOTOR_PIN_F        32    // to DC Motor Driver FIN
#define MOTOR_PIN_R        33    // to DC Motor Driver RIN
#define MOTOR_PWM_F        0     // PWM CHANNEL
#define MOTOR_PWM_R        1     // PWM CHANNEL
#define MOTOR_POWER_MIN    50
#define MOTOR_POWER_MAX    255
//PID
float power=0,I=0,preP=0,preTime;

void setup() {
  M5.begin();
  //Motor設定
  pinMode(MOTOR_PIN_F, OUTPUT);
  pinMode(MOTOR_PIN_R, OUTPUT);
  ledcSetup(MOTOR_PWM_F, 312500, 8); //CHANNEL, FREQ, BIT
  ledcSetup(MOTOR_PWM_R, 312500, 8);
  ledcAttachPin(MOTOR_PIN_F, MOTOR_PWM_F);
  ledcAttachPin(MOTOR_PIN_R, MOTOR_PWM_R);
  //MPU設定
  M5.MPU6886.Init();
  //PID初期化
  preTime = micros(); 
}
void loop() {
  float pitch,roll,yaw,Duty,P,D,now,dt,Time;
  //ロール角取得
  M5.MPU6886.getAhrsData(&pitch,&roll,&yaw);
  //PID計算
  now     = TARGET - roll                 ; // 目標角度から現在の角度を引いて偏差を求める
  if (-20 < now && now < 20) { 
    Time    = micros()                    ;
    dt      = (Time - preTime) / 1000000  ; // 処理時間を求める
    preTime = Time                        ; // 処理時間を記録
    P       = now / 90                    ; // -90~90→-1.0~1.0
    I      += P * dt                      ; // 偏差を積分する
    D       = (P - preP) / dt             ; // 偏差を微分する
    preP    = P                           ; // 偏差を記録する
    power  += KP * P + KI * I + KD * D    ; // 出力を計算する
    if (power < -1) power = -1            ; // →-1.0~1.0
    if (1 < power)  power =  1            ; 
    //Motor駆動
    Duty = (int)((MOTOR_POWER_MAX - MOTOR_POWER_MIN)* abs(power) + MOTOR_POWER_MIN); 
    ledcWrite( MOTOR_PWM_F,(power < 0 ?    0 : Duty) );  
    ledcWrite( MOTOR_PWM_R,(power < 0 ? Duty :    0) ); 
  } else {  // +-20度を越えたら倒れたとみなす
    ledcWrite( MOTOR_PWM_F,0 );
    ledcWrite( MOTOR_PWM_R,0 );
    power = 0;
    I = 0;
  }
  M5.Lcd.setCursor(0, 10);
  M5.Lcd.printf("now:%6.1f",now);
}

#■最後に
姿勢推定のすべてを「getAhrsData」関数に委ねた事により、PIDロジックに集中できるようになりました。PIDロジックや関連係数の作用は未だに腹落ちしていないので、さらに追及してよりスマートなロジックにして行きたいと思います。まだまだ改良は続きます。
#■参考記事
下記記事がきっかけ&参考になりました。制御の詳しい解説もあるので、これからもリファレンスとして参照&活用させていただきます。ありがとうございました。
M5StickCはMadgwickフィルタで倒立振子の夢を見るか？