M5Goを使った倒立振子の試作

はじめに

今回はマイコンを使った倒立振子を試作しました。電子部品の接続を極力簡単にするため、マイコンにはIMU 6軸姿勢センサ（ジャイロ+加速度計）とバッテリを内蔵しているM5Goを使用します。また、サーボドライバユニットを使用することで、サーボモータの配線も単純なものにします。車体の工作もできるだけ簡単なものにしました。

使用したもの

単3電池4本

動作確認

サーボモータ

#include "M5_UNIT_8SERVO.h"

M5_UNIT_8SERVO unit_8servo;

void setup() {
    while (!unit_8servo.begin(&Wire, 21, 22, M5_UNIT_8SERVO_DEFAULT_ADDR)) {
        Serial.println("extio Connect Error");
        delay(100);
    }
    unit_8servo.setAllPinMode(SERVO_CTL_MODE);
}

void loop() {

    for (uint8_t deg = 0; deg <= 180; deg += 20) {
      unit_8servo.setServoAngle(0, deg);
      unit_8servo.setServoAngle(1, 180-deg);
      vTaskDelay(500);
      delay(100);
    }
}

元プログラムはこちら

6軸センサによるX軸回りの倒れ角の測定（ChatGptで作成）

#include <M5Unified.h>
#include <math.h>

static float roll_deg = 0.0f;   // X軸回り（ロール角）[deg]
static uint32_t last_ms = 0;

void setup() {
  auto cfg = M5.config();
  M5.begin(cfg);

  M5.Display.setTextSize(2);
  M5.Display.clear();
  M5.Display.println("M5Go IMU Roll (X-axis)");

  if (!M5.Imu.begin()) {
    M5.Display.println("IMU init failed!");
    while (1) { delay(100); }
  }

  last_ms = millis();
}

void loop() {
  M5.update();

  // dt計算
  uint32_t now = millis();
  float dt = (now - last_ms) / 1000.0f;
  last_ms = now;
  if (dt <= 0) dt = 0.001f;

  // センサ読み取り
  float ax, ay, az;
  float gx, gy, gz; // deg/s
  M5.Imu.getAccel(&ax, &ay, &az);
  M5.Imu.getGyro(&gx, &gy, &gz);

  // 加速度からロール角（X軸回り）を推定
  // roll = atan2(ay, az) ※座標の向きは機種/持ち方で符号が逆になることがあります
  float roll_acc = atan2f(ay, az) * 180.0f / (float)M_PI;

  // ジャイロからロール角を積分（X軸回りは gx を使う）
  float roll_gyro = roll_deg + gx * dt;

  // コンプリメンタリフィルタ（好みで係数を調整）
  const float alpha = 0.98f; // 0.95〜0.99くらいで調整
  roll_deg = alpha * roll_gyro + (1.0f - alpha) * roll_acc;

  // 表示
  M5.Display.setCursor(0, 40);
  M5.Display.fillRect(0, 40, 320, 200, BLACK);

  M5.Display.printf("Roll (X-axis):\n");
  M5.Display.printf("  %.2f deg\n\n", roll_deg);

  M5.Display.printf("acc : %.2f deg\n", roll_acc);
  M5.Display.printf("gyro: %.2f deg/s\n", gx);

  delay(10); // 約100Hz
}

作成した倒立振子

サーボモータ、M5、サーボドライバは両面テープでユニバーサルプレートに貼り付けました。その他はビニールテープで固定しました。「く」の字型のアングルはサーボモータや電池ケースの位置決めのために使用しました。

プログラム（ChatGptで作成）

Aボタンでサーボのオンオフ、Bボタンでキャリブレーションを行います。
はじめに、車体を立てた状態（バランスが取れた位置）でBボタンを押してキャリブレーションを行い、Aボタンを押してサーボモータを駆動させます。

#include <M5Unified.h>
#include <math.h>
#include "M5_UNIT_8SERVO.h"

// ===================== 8SERVOS =====================
M5_UNIT_8SERVO unit_8servo;

static const int I2C_SDA = 21;
static const int I2C_SCL = 22;

static const uint8_t CH_LEFT  = 0;
static const uint8_t CH_RIGHT = 1;

// ===================== 目標・制御 =====================
static const float SETPOINT_DEG = 90.0f;

// ===== PIDゲイン（まずはI=0で調整 → 最後にIを入れる）=====
static const float KP = 5.0f;
static const float KI = 0.2f;
static const float KD = 0.1f;

//static const float DEADBAND_DEG = 1.0f;
static const float DEADBAND_DEG = 0.5f;

// ===== FS90R =====
static const int NEUTRAL_US_L = 1490;
static const int NEUTRAL_US_R = 1500;

static const int MIN_US = 1000;
static const int MAX_US = 2000;

static const float OUT_US_PER_DEG = 8.0f;
static const float OUT_US_PER_DPS = 8.0f;

// I項（積分）制限（アンチワインドアップ）
static const float I_TERM_LIMIT = 150.0f; // us 相当

static const bool INVERT_LEFT  = false;
static const bool INVERT_RIGHT = true;

// ===================== 姿勢推定 =====================
static float roll_deg = 0.0f;
static float roll_offset_deg = 0.0f;
static float gx_dps = 0.0f;

static uint32_t last_ms = 0;
static bool armed = false;

// ===== I制御用 =====
static float error_i = 0.0f;

// ===================== Utility =====================
static float clampf(float v, float lo, float hi) {
  if (v < lo) return lo;
  if (v > hi) return hi;
  return v;
}

void setStop() {
  unit_8servo.setServoPulse(CH_LEFT,  NEUTRAL_US_L);
  unit_8servo.setServoPulse(CH_RIGHT, NEUTRAL_US_R);
}

// ===================== Roll推定 =====================
void updateRollEstimate(float dt) {
  float ax, ay, az;
  float gx, gy, gz;
  M5.Imu.getAccel(&ax, &ay, &az);
  M5.Imu.getGyro(&gx, &gy, &gz);

  gx_dps = gx;

  float roll_acc  = atan2f(ay, az) * 180.0f / M_PI;
  float roll_gyro = roll_deg + gx * dt;

  const float alpha = 0.98f;
  roll_deg = alpha * roll_gyro + (1.0f - alpha) * roll_acc;
}

// ===================== Bボタンキャリブレーション =====================
void calibrateByButton() {
  setStop();
  error_i = 0.0f;   // ★積分リセット

  M5.Display.fillRect(0, 60, 320, 120, BLACK);
  M5.Display.setCursor(0, 60);
  M5.Display.println("Calibrating...");
  M5.Display.println("Hold upright");

  const int interval_ms = 10;
  const int warmup = 50;
  const int N = 150;
  float dt = interval_ms / 1000.0f;

  for (int i = 0; i < warmup; i++) {
    updateRollEstimate(dt);
    delay(interval_ms);
  }

  float sum = 0.0f;
  for (int i = 0; i < N; i++) {
    updateRollEstimate(dt);
    sum += roll_deg;
    delay(interval_ms);
  }

  float roll_avg = sum / N;
  roll_offset_deg = SETPOINT_DEG - roll_avg;

  M5.Display.fillRect(0, 60, 320, 120, BLACK);
  M5.Display.setCursor(0, 60);
  M5.Display.println("Cal DONE");
  M5.Display.printf("avg: %.2f\n", roll_avg);
  M5.Display.printf("off: %.2f\n", roll_offset_deg);
  delay(800);
}

// ===================== setup =====================
void setup() {
  auto cfg = M5.config();
  M5.begin(cfg);

  Serial.begin(115200);
  delay(200);

  M5.Display.clear();
  M5.Display.setTextSize(2);
  M5.Display.println("M5Core2 Inverted");
  M5.Display.println("BtnA: ARM");
  M5.Display.println("BtnB: CAL(90)");

  if (!M5.Imu.begin()) {
    while (1) delay(100);
  }

  while (!unit_8servo.begin(&Wire, I2C_SDA, I2C_SCL,
                            M5_UNIT_8SERVO_DEFAULT_ADDR)) {
    delay(100);
  }
  unit_8servo.setAllPinMode(SERVO_CTL_MODE);

  setStop();
  last_ms = millis();
}

// ===================== loop =====================
void loop() {
  M5.update();

  // ARM切替
  if (M5.BtnA.wasPressed()) {
    armed = !armed;
    error_i = 0.0f; // ★切替時に積分リセット
    if (!armed) setStop();
  }

  // Bボタン：キャリブレーション
  if (M5.BtnB.wasPressed()) {
    bool was_armed = armed;
    armed = false;
    calibrateByButton();
    armed = was_armed;
  }

  float dt = (millis() - last_ms) / 1000.0f;
  last_ms = millis();
  if (dt <= 0) dt = 0.001f;

  updateRollEstimate(dt);

  float roll_cal = roll_deg + roll_offset_deg;
  float error = SETPOINT_DEG - roll_cal;

  if (fabsf(error) < DEADBAND_DEG) error = 0.0f;

  // ===================== PID =====================
  float up = KP * error * OUT_US_PER_DEG;

  // ★I制御（小さな誤差のときのみ積分）
  if (armed && fabsf(error) < 10.0f) {
    error_i += error * dt;
    error_i = clampf(error_i, -I_TERM_LIMIT, I_TERM_LIMIT);
  }

  float ui = KI * error_i;

  float ud = KD * gx_dps * OUT_US_PER_DPS;

  float u_us = up + ui - ud;

  int cmdL = NEUTRAL_US_L + (INVERT_LEFT  ? -u_us : u_us);
  int cmdR = NEUTRAL_US_R + (INVERT_RIGHT ? -u_us : u_us);

  cmdL = (int)clampf(cmdL, MIN_US, MAX_US);
  cmdR = (int)clampf(cmdR, MIN_US, MAX_US);

  if (armed) {
    unit_8servo.setServoPulse(CH_LEFT,  cmdL);
    unit_8servo.setServoPulse(CH_RIGHT, cmdR);
  } else {
    setStop();
  }

  delay(10);
}

動作

ストレッチマット上ですが、プログラムのKP, KI, KDを調整して、倒立振子を自立させることができました。キャリブレーションがうまく行くと、何分も自立させることができました。ただし、モータの性能に対して車体が重いためか、外乱等により車体の傾きが大きくなると自立できなくなります。

まとめ

簡単な組み立てで作れる倒立振子を紹介しました。面倒なのはPID値の調整ですが、それも含めて1日でできると思います。電池ケースにサーボモータやM5を貼り付けて車体にすれば、小型化できるため、制御が容易になると考えています。