Arduinoを使って 1モータ・1軸のシンプルな倒立振子ロボットを作りました

////////////////////////////////////////////////////////////////////////
//GY-521(加速度・角速度センサ)の加速度と角速度のオフセット値を計測するスケッチ
////////////////////////////////////////////////////////////////////////

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

long accelX_sum = 0;
long accelY_sum = 0;
long accelZ_sum = 0;
long gyroX_sum = 0;
long gyroY_sum = 0;
long gyroZ_sum = 0;

void setup() {
  Serial.begin(9600);       // シリアルモニタを9600bpsで開始
  Wire.begin();             // I2C開始
  mpu.initialize();         // MPU6050初期化

  Serial.println("静止状態で1000回データ取得中...");
}

void loop() {
  // 1000回サンプリング
  for (int i = 0; i < 1000; i++) {
    int16_t ax, ay, az;
    int16_t gx, gy, gz;

    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    accelX_sum += ax;
    accelY_sum += ay;
    accelZ_sum += az;
    gyroX_sum += gx;
    gyroY_sum += gy;
    gyroZ_sum += gz;

    delay(2); // サンプリング間隔（約2ms）
  }

  // 平均値計算
  long accelX_avg = accelX_sum / 1000;
  long accelY_avg = accelY_sum / 1000;
  long accelZ_avg = accelZ_sum / 1000;
  long gyroX_avg = gyroX_sum / 1000;
  long gyroY_avg = gyroY_sum / 1000;
  long gyroZ_avg = gyroZ_sum / 1000;

  // 結果出力
  Serial.println("=== 平均値 ===");
  Serial.print("Accel X: "); Serial.println(accelX_avg);
  Serial.print("Accel Y: "); Serial.println(accelY_avg);
  Serial.print("Accel Z: "); Serial.println(accelZ_avg);
  Serial.print("Gyro X: "); Serial.println(gyroX_avg);
  Serial.print("Gyro Y: "); Serial.println(gyroY_avg);
  Serial.print("Gyro Z: "); Serial.println(gyroZ_avg);

  // 一度だけ計算して終了
  while (1);
}

////////////////////////////////////////////////////////////////////////
// 倒立振子 バランス制御
////////////////////////////////////////////////////////////////////////

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// -------------------------
// 制御周期の設定
// -------------------------
unsigned long sampleTime = 5000; // 制御周期（マイクロ秒）。5ms = 200Hz
unsigned long prevMicros = 0;

// -------------------------
// デバッグ設定
// -------------------------
#define DEBUG0 0
#define DEBUG1 1
#define DEBUG2 2
#define DEBUG3 3
#define DEBUG4 4

#define DEBUG_MODE DEBUG1

// -------------------------
// 制御パラメータ
// -------------------------
float targetAngle = -0.75;
int MAX_PWM = 255;
float FALL_ANGLE = 60.0;

// PD制御ゲイン
float Kp = 55.0;
float Kd = 1.20;

// ジャイロオフセット
const int gyroOffsetX = -441;
const int gyroOffsetY = 202;
const int gyroOffsetZ = 28;

// TB6612FNG ピン設定
const int AIN1 = 2;
const int AIN2 = 3;
const int PWMA = 5;

// -------------------------
int16_t ax, ay, az;
int16_t gx, gy, gz;

float angle = 0;
unsigned long lastDebug = 0;

// -------------------------
// モーター制御（デッドゾーン補正入り）
// -------------------------
void motorDrive(int pwm)
{
    pwm = constrain(pwm, -MAX_PWM, MAX_PWM);

    // デッドゾーン補正
    const int DEADZONE = 60;

    if (pwm > 0 && pwm < DEADZONE) {
        pwm = DEADZONE;
    } else if (pwm < 0 && pwm > -DEADZONE) {
        pwm = -DEADZONE;
    }

    // 完全停止
    if (pwm == 0) {
        digitalWrite(AIN1, LOW);
        digitalWrite(AIN2, LOW);
        analogWrite(PWMA, 0);
        return;
    }

    // 通常駆動
    if (pwm > 0) {
        digitalWrite(AIN1, HIGH);
        digitalWrite(AIN2, LOW);
        analogWrite(PWMA, pwm);
    } else {
        digitalWrite(AIN1, LOW);
        digitalWrite(AIN2, HIGH);
        analogWrite(PWMA, -pwm);
    }
}

// -------------------------
// 角度計算（相補フィルタ）
// -------------------------
float getAngle(float dt)
{
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // ジャイロ値の補正
    float gyroRate = -((gy - gyroOffsetY) / 131.0);
    float accelAngle = atan2((float)ax, (float)az) * 180.0 / PI;

    // dt を用いた相補フィルタ
    angle = 0.98 * (angle + gyroRate * dt) + 0.02 * accelAngle;

    return angle;
}

// -------------------------
void setup()
{
    Serial.begin(115200);

    Wire.begin();
    mpu.initialize();

    pinMode(AIN1, OUTPUT);
    pinMode(AIN2, OUTPUT);
    pinMode(PWMA, OUTPUT);

    prevMicros = micros();
}

// -------------------------
void loop()
{
    unsigned long currentMicros = micros();

    // 指定した sampleTime ごとに実行
    if (currentMicros - prevMicros >= sampleTime) {
        float dt = (currentMicros - prevMicros) / 1000000.0;
        prevMicros = currentMicros;

        float ang = getAngle(dt);
        float angError = ang - targetAngle;

        // 転倒時は停止
        if (abs(angError) > FALL_ANGLE) {
            motorDrive(0);
            return;
        }

        // PD 制御
        // Kd項には応答性の良いジャイロ直値（角速度）を使用
        float gyroRate = -((gy - gyroOffsetY) / 131.0);
        float control = Kp * angError + Kd * gyroRate;

        int pwm = (int)control;
        motorDrive(pwm);

        // デバッグ出力（50ms周期）
        if (millis() - lastDebug >= 50) {
            lastDebug = millis();

            #if DEBUG_MODE == DEBUG1
                Serial.print("Angle: "); Serial.print(ang);
                Serial.print("  Control: "); Serial.print(control);
                Serial.print("  PWM: "); Serial.println(pwm);

            #elif DEBUG_MODE == DEBUG2
                Serial.print(ang); Serial.print(",");
                Serial.print(control); Serial.print(",");
                Serial.println(pwm);

            #elif DEBUG_MODE == DEBUG3
                Serial.print("Angle: "); Serial.print(ang);
                Serial.print("  Ax: "); Serial.print(ax);
                Serial.print("  Ay: "); Serial.print(ay);
                Serial.print("  Az: "); Serial.print(az);
                Serial.print("  Gx: "); Serial.print(gx);
                Serial.print("  Gy: "); Serial.print(gy);
                Serial.print("  Gz: "); Serial.println(gz);

            #elif DEBUG_MODE == DEBUG4
                Serial.print(ang); Serial.print(",");
                Serial.print(ax); Serial.print(",");
                Serial.print(ay); Serial.print(",");
                Serial.print(az); Serial.print(",");
                Serial.print(gx); Serial.print(",");
                Serial.print(gy); Serial.print(",");
                Serial.println(gz);
            #endif
        }
    }
}