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

#■はじめに
###【注意】MStickC IMUはMPU6886を対象としています。

###　倒立振子って、なんか立ち姿が神ってて(?)憧れますよね。でも自分で作るとなると、ハード・ソフト両面でかなりハードルが高いですよね。そこで今回は、そんな夢の倒立振子を簡単すぎるほど簡単に安価に作成する方法を紹介します。あまり堅苦しい事は考えずに、とりあえずPID制御ぽい倒立振子をゆるく立たせてみた的な向きの記事です。なので、あくまでもゆるふわで見て下さい。でも、立派にバランスをとって立ちます!!

###　極限まで安価・簡単にしました。ネジ類は使いません、3か所を強力両面テープで貼り付けるだけです。Motor Driver基板への半田付けはありますが、そんなの半田付けのうちに入りませんよねw

###　周辺部品を既に所持していると思われるIoTやゆるメカトロ界隈の方であれば、250円のギャードモーターを2個買い増しするだけで実質500円でできます。プログラムはコメントや変数宣言を除き、ロジック部分だけだと100Step未満という簡単さです。

###　コードはあちこちからのツギハギで、実は作った本人も全ては理解できていません。PIDも、本当に効いているのかすらわかりません。でも、ぽく立っています。そんな中途半端な代物なのですが、一人でも多くの方が倒立振子を立たせてエモってもらえればと思い、思い切って放流しました。みなさんの知識やアィディアで、よりシンプルに楽しめるものに仕上がって行けばうれしいなと。どんどん改良して、情報シェアをお願いします。

#■まずは立派な立ち姿を見てやって下さい

実質500円&100StepPID制御による超簡単倒立振子完成。#M5StickC × 内蔵MPU6886 × DC Motor Driver BD6211F × ギャードモーター FM90。Qiita投稿準備中。 pic.twitter.com/ROSSorN5Pu

— Google Homer (@google_homer_) February 16, 2020

#■使用部品(クリックで購入サイト)
①FEETECH ギヤードモーター FM90 × 2個 (250円/個)
安価、小型、四角、短ケーブルなFM90を選択しましたが、遊びの少ない小型ギヤードモーターならよいです。TAMIYA工作ギヤボックスは、遊びが大きくダメでした。また、サーボもPWMのパルス幅で勝負している関係で、レスポンスが極端に悪く調整が激ムズで現実的ではありませんでした。
②車輪
サーボSG90等付属の円形ホーンをホイールとして流用します(FM90にはついてませんでした)。安定のため、ホーンより少し径の大きいテーブル用滑り止め(100均)をタイヤとして貼り付けました。
③DC Motor Driver BD6211F
2台のモーターを並列にし1チャンネルで動かします。Hブリッジ系であればよく、TB6612FNGでも動作確認できています。
④ヘッダピン 2×2 片側ロング
FM90の端子が2ピンメスなので、これでDC Motor Driverの基板との中継をします。
⑤GROVEケーブル
1本あればよいです。
#■接続図
###6軸センサーはM5StickC内蔵のMPU6886を使用します。
2台のモーターの+/-を逆にして2段ヘッダピンで並列接続します。GROVE端子の片側は切断し、直接基板に半田付けしました。配線は下記参考に、お好みの形でどうぞ。

#■車体の組み立て
###ネジ類は使いません、3か所を強力両面テープで貼り付けるだけです。完全にこの通りである必要はありません。雰囲気で作り上げて下さい。後述するパラメータ調整で何とでもります(たぶん)。




###今回は簡単さを優先して両面テープでゆるく作りましたが、全体をプレート等で固定する構造にすることで、より安定して立てるようになると思います。

#■コード
Arduino IDEでやりました。ちょっとあれ(?)なので、説明は割愛します。たったの142Stepかつ実ロジックはシンプルなのでたぶん追えると思います。

#include <M5StickC.h>

//PIDパラメータ調整
#define OFFSET             91.5                    // 重心 後方-←90°→+前方
#define COEF_P             35.0                    // Propotional 
#define COEF_I             1.5                     // Integral
#define COEF_D             50.0                    // Differential

//Motor
#define LED_PIN            10                      // 内蔵LED
#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
  
//MPU6886 https://github.com/m5stack/M5StickC/blob/master/src/utility/MPU6886.h
#define MPU6886_AFS        M5.MPU6886.AFS_2G       // Ascale [g]      (±2,4,8,16)
#define MPU6886_GFS        M5.MPU6886.GFS_1000DPS  // Gscale [deg/s]  (±250,500,1000,200)
#define DPS                1000                    // Gscale [deg/s]
#define CALIBRATION_COUNT  1000

//Function
#define min(a,b)           ((a) < (b) ? (a) : (b))
#define max(a,b)           ((a) > (b) ? (a) : (b))
  
//キャリブレ結果
double offsetX;              //Gyro X
float  base_angle;           //Accl X
//ローテーション結果
float dpsX;                  //Gyro X
float angleX;                //Accl X
//積分
double gyro_angle_x = 0;
float  preInterval  = 0;

void setup() {
  //初期化
  M5.begin();
  M5.Lcd.setRotation(1);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(10, 0);
  M5.Lcd.println("<1Drive/2DC Motor Type>");
  //Motor設定
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);
  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();
  M5.MPU6886.SetGyroFsr(MPU6886_GFS);
  M5.MPU6886.SetAccelFsr(MPU6886_AFS);
  //キャリブレーション
  calibration();
  M5.Lcd.setCursor(0, 20);
  M5.Lcd.printf(" base_angle: %.1f", base_angle);
}

void loop() {
  static  int   Duty;
  float         power, powerP, powerD;
  static float  powerI = 0;
  double        angle;

  //MPUローテーション
  calcRotation();
  //モーター制御
  angle = angleX;
  if ((angle - OFFSET > -15) && (angle - OFFSET < 15)) {    // ３０度以下だけモーター駆動
    powerP = (angle - OFFSET ) / 90;                        // angle=0～180  → -1～1
    powerI += powerP;
    powerD = dpsX / DPS;                                    // 角速度 → -1～1
    power = powerP * COEF_P + powerI * COEF_I + powerD * COEF_D;
    power = max(-1, min(1, power));                         // -1～1
    //モーター駆動
    Duty = (int)(200* abs(power) + 50); 
    ledcWrite( MOTOR_PWM_F,(power < 0 ?    0 : Duty) );  
    ledcWrite( MOTOR_PWM_R,(power < 0 ? Duty :    0) ); 
    digitalWrite(LED_PIN, HIGH);
  } else {                                                  // 転倒したら停止
    ledcWrite(MOTOR_PWM_F, 0);
    ledcWrite(MOTOR_PWM_R, 0);
    powerI = 0;
    digitalWrite(LED_PIN, LOW);
  }
  M5.update();
  if (M5.BtnB.wasReleased()) {
    M5.Lcd.print('B');
    ESP.restart();
    while (true) ;
  }
}

//キャリブレーション関数
void calibration() {
  float raw_gyro_x, raw_gyro_y, raw_gyro_z;
  float raw_acc_x, raw_acc_y, raw_acc_z;
  float acc_x = 0, acc_y = 0, acc_z = 0;
  offsetX = 0;
  for (int i = 0; i < CALIBRATION_COUNT; i++) {
    M5.MPU6886.getGyroData(&raw_gyro_x, &raw_gyro_y, &raw_gyro_z);
    M5.MPU6886.getAccelData(&raw_acc_x, &raw_acc_y, &raw_acc_z);
    //ベースプログラムとM5StickCの組付け位置の違いによる座標系変換含む
    offsetX += -raw_gyro_x;
    acc_x   += -raw_acc_x;
    acc_y   +=  raw_acc_y;
    acc_z   += -raw_acc_z;
  }
  //gyro offset
  offsetX /= CALIBRATION_COUNT;
  //加速度から角度を算出
  acc_x   /= CALIBRATION_COUNT;
  acc_y   /= CALIBRATION_COUNT;
  acc_z   /= CALIBRATION_COUNT;   
  base_angle = (atan2(acc_y, acc_z + abs(acc_x)) * 360 / 2.0 / PI) + 180;
}

//MPUローテーション関数
void calcRotation() {
  float raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
  float acc_angle_x;
  M5.MPU6886.getGyroData(&raw_gyro_x, &raw_gyro_y, &raw_gyro_z);
  M5.MPU6886.getAccelData(&raw_acc_x, &raw_acc_y, &raw_acc_z);
  //ベースプログラムとM5StickCの組付け位置の違いによる座標系変換
  raw_gyro_x *= -1;
  raw_acc_x  *= -1;
  raw_acc_z  *= -1;
  //
  acc_angle_x = (atan2(raw_acc_y, raw_acc_z + abs(raw_acc_x)) * 360 / 2.0 / PI) + 180;
  dpsX = raw_gyro_x - offsetX;
  //積分
  float now_time = millis();
  float interval = now_time - preInterval;  //前回からの経過時間
  preInterval = now_time;
  gyro_angle_x += dpsX * (interval * 0.000999999);
  //相補フィルター
  angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x) + base_angle;
}

#■パラメータの調整方法
たぶん前述のとおり組み立てたとしても個体差は必ずあるはずです。その場合は、うまく立たせるためにはプログラム中のパラメータの調整が必要になります。**この調整は、かなりの曲者で、自分の場合は何とか立てるようになるまでパラメータ調整だけで丸半日かかってしまいました。**いまだに、どうだったらどの項目をどうすればよいか明確に把握できていないのですが、とりあえずゆるく解説しておきます。コードの下記のdefineの値を書き換える作業になります。トライアンドエラーで根気よく進めて下さい。

//PIDパラメータ調整
#define OFFSET             91.5                    // 重心 後方-←90°→+前方
#define COEF_P             35.0                    // Propotional 
#define COEF_I             1.5                     // Integral
#define COEF_D             50.0                    // Differential

インパクトの大きかった順に記載します。
①OFFSET　(重心、結構効く)
・直立を90とし、重心が前の場合+、後ろの場合-する。
・80～100、0.5刻みぐらいで調整。
②COEF_D　(傾いた場合の復元力みたいな作用、結構効く)
・0から1刻みで上げていき、前後に振れる荒ぶりを抑えていく。(～200ぐらい)
③COEF_I
・0～5、0.1刻みぐらいで調整。動きの小さな変化はあったが、作用把握できなかった。
④COEF_P
・よくわからない、35.0からいじってない。

#■立たせ方(上の動画も参考にして下さい)
① 床に水平に寝かせ、M5StickC本体が床対して水平になるよう手で保持します。
② M5StickCのDisplayの横のBボタンを押します。
③ 赤ランプがつき、Displayにbase_angleの値(0～360)が表示される。
④ base_angleが0～1ぐらいの範囲である事を確認したら、そお～つと手で立たせます。
うまく行けば、これで自立します。最初は何度も練習必要です。がんばりましょう。
寝かせた際に頭が下がりすぎると値が360近傍になり、起こしても何の反応もないことがあります。手で少し頭を上げると0以上になり正常に反応します。
⑤ base_angleが上記以外の場合は①から繰り返します。
(注意)積分が悪さをしているのだと思いますが、放置しておくと勝手に車輪が回り始める事があります。
##うまく立ったら、その時のbase_angle値を覚えておき、次もその値になるまでBボタンを押して開始するとよいです。

#■最後に
###根気強くがんばりましょう。動いたら、倒立振子フリークを増やすためにSNS拡散をお願いします。

#■参考にした情報
この倒立振子の制作にあたっては、おおたfab さんの素人でもロボット作りたい！　バランスカー編第3回に参加させていただいた際に提供いただいたBalanceCar Kitを参考にさせて頂きました。そのほかにも、とてもおもしろいハンズオンを開催しているのでアクセスしてみて下さい。おおたfabさん、ありがとうございました。