1
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@_Moony

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

Last updated at Posted at 2026-01-12

はじめに

M5StickCPlusを使った小型の倒立振子を試作しました。M5本体のバッテリで2個のサーボモータFS90Rを駆動させます。

部品

倒立振子の作成

  • FS90Rにタイヤを取り付け
  • 2個のFS90Rを両面テープで貼り付け
  • 以下のように結線
    • コネクタ付ケーブル（4本）を適当な長さに切断
      　（ここでは、コネクタ付ケーブルの色は黒、赤、白、黄とする）
    • 2個のサーボモータのケーブルをそれぞれ途中で切断
      　（2個のサーボモータをサーボモータA、サーボモータBとする）
    • 切断したケーブルの切断部を皮をむきして、次のようにはんだ付け
      　　サーボモータAの黒線　−　サーボモータBの黒線　−　コネクタ付ケーブルの黒線
      　　サーボモータAの赤線　−　サーボモータBの赤線　−　コネクタ付ケーブルの赤線
      　　サーボモータAの橙線　−　コネクタ付ケーブルの白線
      　　サーボモータBの橙線　−　コネクタ付ケーブルの黄線
    • コネクタ付ケーブルをM5StickCPlusに次のように接続
      　　コネクタ付ケーブルの黒線　−　M5StickCPlusのGND
      　　コネクタ付ケーブルの赤線　−　M5StickCPlusの5V➡
      　　コネクタ付ケーブルの白線　−　M5StickCPlusのG26
      　　コネクタ付ケーブルの黄線　−　M5StickCPlusのG25

上記の接続を図示したものです。
M5StickCPlus_倒立振子_ブレッドボード.jpg

以下に組立後の倒立振子を示します。

IMG_20260112_160441.jpg
IMG_20260112_160449.jpg

倒立振子の動作の様子です。ストレッチマット上で自立することができました。
VID_20260112_165002(1).gif

スケッチは以下のとおりです（ChatGptで作成）。

#include <M5Unified.h>
#include <math.h>
#include <ESP32Servo.h>
#include <WiFi.h>   // WiFi OFF用

// ===================== GPIO直結サーボ =====================
static const int SERVO_L_PIN = 26;
static const int SERVO_R_PIN = 25;

Servo servoL;
Servo servoR;

// ===================== 目標・制御 =====================
// ★Bボタンで「そのときの姿勢（ほぼ垂直）」が 0deg になるようにオフセットを決める
static const float SETPOINT_DEG = 0.0f;

// ===== PIDゲイン（“細かい補正”寄り）=====
static const float KP = 4.5f;
static const float KI = 0.0f;
static const float KD = 0.08f;   // ★変更：0.2 → 0.08（Dを弱めて暴れにくく）

// ★変更：小さな傾きでも反応開始（0.10 → 0.05）
static const float DEADBAND_DEG = 0.03f;

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

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

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

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

// 左右の回転方向が合っている前提（合わなければtrue/falseを入替）
static const bool INVERT_LEFT  = false;
static const bool INVERT_RIGHT = true;

// ===================== 姿勢推定（X軸回り：roll） =====================
// 変数名は互換のため pitch_* を維持（中身はroll）
static float pitch_deg = 0.0f;
static float pitch_offset_deg = 0.0f;
static float gy_dps = 0.0f;     // 互換のため名称維持（中身はgx）

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

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

// ===================== 落ち対策（追加） =====================
static const uint32_t SOFTSTART_NEUTRAL_MS = 200;
static const uint32_t SOFTSTART_RAMP_MS    = 300;
static uint32_t armed_since_ms = 0;

// 出力の急変を抑える（サーボパルスのスルーレート制限）
static const int SLEW_US_PER_LOOP = 14;
static int last_cmdL = NEUTRAL_US_L;
static int last_cmdR = NEUTRAL_US_R;

// 低電圧停止（読める場合のみ有効）
static const float VBAT_CUTOFF = 3.55f;

// ===================== 表示サイズ（StickC Plus対応） =====================
static int W = 0;
static int H = 0;

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

static int clampi(int v, int lo, int hi) {
  if (v < lo) return lo;
  if (v > hi) return hi;
  return v;
}

static int slew(int target, int current, int step) {
  if (target > current + step) return current + step;
  if (target < current - step) return current - step;
  return target;
}

void setStop() {
  last_cmdL = NEUTRAL_US_L;
  last_cmdR = NEUTRAL_US_R;
  servoL.writeMicroseconds(NEUTRAL_US_L);
  servoR.writeMicroseconds(NEUTRAL_US_R);
}

// 可能なら電圧を読む（読めない機種/環境では -1）
static float readBatteryVoltageOrNeg1() {
  // コンパイルエラーになる環境では return -1; にしてください
  return M5.Power.getBatteryVoltage();
}

// ===================== 姿勢推定（X軸回り） =====================
// roll（加速度由来）：atan2(ay, az)
// roll（ジャイロ）：gx を積分
void updatePitchEstimate(float dt) {
  float ax, ay, az;
  float gx, gy, gz;
  M5.Imu.getAccel(&ax, &ay, &az);
  M5.Imu.getGyro(&gx, &gy, &gz);

  // D項に使うのはX軸角速度
  gy_dps = gx;

  // X軸回り角（roll）
  float roll_acc = atan2f(ay, az) * 180.0f / (float)M_PI;

  // ジャイロ積分もX軸で統一
  float roll_gyro = pitch_deg + gx * dt;

  // ★変更：加速度を少し強めに混ぜる（0.98 → 0.96）
  const float alpha = 0.96f;
  pitch_deg = alpha * roll_gyro + (1.0f - alpha) * roll_acc;
}

// ===================== Bボタンキャリブレーション =====================
// 「今の姿勢（ほぼ垂直）」を 0deg としてオフセット設定
void calibrateByButton() {
  setStop();
  error_i = 0.0f;

  M5.Display.fillRect(0, 0, W, H, BLACK);
  M5.Display.setCursor(0, 0);
  //M5.Display.setTextSize(1);
  M5.Display.setTextSize(2);
  M5.Display.println("Calibrating...");
  M5.Display.println("Hold NEAR VERTICAL");
  M5.Display.println("(this pose => 0 deg)");

  const int interval_ms = 10;
  const int warmup = 40;
  const int N = 120;
  float dt = interval_ms / 1000.0f;

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

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

  float avg = sum / N;

  pitch_offset_deg = SETPOINT_DEG - avg;

  M5.Display.fillRect(0, 0, W, H, BLACK);
  M5.Display.setCursor(0, 0);
  M5.Display.println("Cal DONE");
  M5.Display.printf("avg: %.2f\n", avg);
  M5.Display.printf("off: %.2f\n", pitch_offset_deg);
  delay(600);
}

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

  WiFi.mode(WIFI_OFF);
  btStop();

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

  M5.Display.setRotation(1);
  M5.Display.setBrightness(200);
  M5.Display.setTextColor(WHITE, BLACK);
  W = M5.Display.width();
  H = M5.Display.height();
  M5.Display.clear(BLACK);
  M5.Display.setCursor(0, 0);
  //M5.Display.setTextSize(1);
  M5.Display.setTextSize(2);
  M5.Display.println("M5StickC Plus");
  M5.Display.println("Inverted Pendulum");
  M5.Display.println("X-axis (roll) ctrl");
  M5.Display.println("BtnA: ARM ON/OFF");
  M5.Display.println("BtnB: CAL (VERT=0)");
  delay(300);

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

  servoL.setPeriodHertz(50);
  servoR.setPeriodHertz(50);
  servoL.attach(SERVO_L_PIN, MIN_US, MAX_US);
  servoR.attach(SERVO_R_PIN, MIN_US, MAX_US);

  setStop();
  last_ms = millis();
}

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

  // ARM切替
  if (M5.BtnA.wasPressed()) {
    armed = !armed;
    error_i = 0.0f;
    if (armed) {
      armed_since_ms = millis();
      setStop();
    } else {
      setStop();
    }
  }

  // Bボタン：キャリブレーション（ほぼ垂直を0degに）
  if (M5.BtnB.wasPressed()) {
    bool was_armed = armed;
    armed = false;
    setStop();
    calibrateByButton();
    armed = was_armed;
    if (armed) armed_since_ms = millis();
  }

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

  updatePitchEstimate(dt);

  // 低電圧停止（読める場合のみ）
  float vbat = readBatteryVoltageOrNeg1();
  if (armed && vbat > 0.0f && vbat < VBAT_CUTOFF) {
    armed = false;
    setStop();
  }

  float pitch_cal = pitch_deg + pitch_offset_deg;
  float error = SETPOINT_DEG - pitch_cal;

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

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

  const float I_ERR_LIMIT = 2.0f;
  const float I_GY_LIMIT  = 4.0f;

  if (armed && fabsf(error) < I_ERR_LIMIT && fabsf(gy_dps) < I_GY_LIMIT) {
    error_i += error * dt;
    error_i = clampf(error_i, -I_TERM_LIMIT, I_TERM_LIMIT);
  }

  float ui = KI * error_i;

  float ud = KD * gy_dps * OUT_US_PER_DPS;

  float u_us = up + ui - ud;

  const float U_LIMIT_US = 220.0f;   // 160〜260で調整
  u_us = clampf(u_us, -U_LIMIT_US, U_LIMIT_US);

  // ★変更：微小出力を殺しすぎない（10 → 5）
  const float U_DEAD_US = 5.0f;
  if (fabsf(u_us) < U_DEAD_US) u_us = 0.0f;

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

  cmdL = clampi(cmdL, MIN_US, MAX_US);
  cmdR = clampi(cmdR, MIN_US, MAX_US);

  // ===================== ソフトスタート＋スルーレート =====================
  if (armed) {
    uint32_t t = millis() - armed_since_ms;

    if (t < SOFTSTART_NEUTRAL_MS) {
      cmdL = NEUTRAL_US_L;
      cmdR = NEUTRAL_US_R;
    } else if (t < SOFTSTART_NEUTRAL_MS + SOFTSTART_RAMP_MS) {
      float mix = (float)(t - SOFTSTART_NEUTRAL_MS) / (float)SOFTSTART_RAMP_MS;
      cmdL = (int)(NEUTRAL_US_L + mix * (cmdL - NEUTRAL_US_L));
      cmdR = (int)(NEUTRAL_US_R + mix * (cmdR - NEUTRAL_US_R));
    }

    cmdL = slew(cmdL, last_cmdL, SLEW_US_PER_LOOP);
    cmdR = slew(cmdR, last_cmdR, SLEW_US_PER_LOOP);
    last_cmdL = cmdL;
    last_cmdR = cmdR;

    servoL.writeMicroseconds(cmdL);
    servoR.writeMicroseconds(cmdR);
  } else {
    setStop();
  }

  // ===================== 表示 =====================
  static uint32_t last_disp = 0;
  if (millis() - last_disp > 150) {
    last_disp = millis();
    M5.Display.fillRect(0, 0, W, H, BLACK);
    M5.Display.setCursor(0, 0);
    //M5.Display.setTextSize(1);
    M5.Display.setTextSize(2);
    M5.Display.printf("ARM:%s\n", armed ? "ON " : "OFF");
    M5.Display.printf("roll: %+.2f\n", pitch_deg);
    M5.Display.printf("off : %+.2f\n", pitch_offset_deg);
    M5.Display.printf("cal : %+.2f\n", pitch_cal);
    M5.Display.printf("err : %+.2f\n", error);
    M5.Display.printf("gx  : %+.2f\n", gy_dps);
    M5.Display.printf("L/R : %d/%d\n", last_cmdL, last_cmdR);
    if (vbat > 0.0f) M5.Display.printf("Vbat: %.2f\n", vbat);
    else             M5.Display.printf("Vbat: n/a\n");
  }

  delay(10);
}
1
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?