Help us understand the problem. What is going on with this article?

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

■はじめに

以前書いた下記の記事のコードをさらに簡略化し、60Stepまで大幅短縮したものです。車体は全く同じなので下記の記事を参考にして下さい。

実質500円&100Stepで作る超簡単「 ゆるメカトロ的 M5StickC 倒立振子」

■以前の記事からの主な変更点

①姿勢推定ロジックを「getAhrsData」関数で置き換え

以前の記事では加速度とジャイロから姿勢推定計算を行っていましたが、これをM5StickCが提供している下記の関数で置き換えることで一発で姿勢の取得ができ、大幅にコードを簡略化できました。この関数は内部で、「カルマンフィルタ」よりも精度がよいと噂されている「Madgwickフィルタ」を呼んでいるようです。

M5.MPU6886.getAhrsData(&pitch,&roll,&yaw);

車体へのM5StickCの組付け形態からすると、車体の前後運動はpitchに反応があるのかなと思ったのですが、実際はrollに反応があったのでそれを使いました。理由は不明です。

②キャリブレ操作不要になり、いきなり立たせられます

■倒立動画


バッテリーの持ちの短さが悩み。

■コード

たったの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フィルタで倒立振子の夢を見るか?

iotlt
IoT縛りの勉強会です。 毎月イベントを実施しているので是非遊びに来てください! 登壇者を中心にQiitaでも情報発信していきます。 https://iotlt.connpass.com
https://iotlt.connpass.com/
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした