自己バランスロボットを作りました Vol #3 – プログラミング

////////////////////////////////////////////////////////////////////////
// 連続回転サーボモータをwriteMicroseconds()関数を使って回転させるスケッチ
////////////////////////////////////////////////////////////////////////

#include <Servo.h>

Servo servo1;  // サーボ1（ピン5）
Servo servo2;  // サーボ2（ピン6）

void setup() {

  Serial.begin(9600); //シリアル通信を9600bpsで開始

  servo1.attach(5);
  servo2.attach(6);
}

void loop() {
  // SG90-HVの基準値は1500µs（停止）

  int speedPulse = 1500; // 回転速度を設定

  servo1.writeMicroseconds(speedPulse);
  servo2.writeMicroseconds(speedPulse);

  // 回転速度設定値を表示
  Serial.print("SpeedPulse: ");
  Serial.println(speedPulse);

}

////////////////////////////////////////////////////////////////////////
//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初期化

  if (!mpu.testConnection()) {
    Serial.println("MPU6050接続エラー");
    while (1);
  }

  Serial.println("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>
#include <Servo.h>

// --- ピン設定 ---
#define TRIG_PIN 2
#define ECHO_PIN 3
#define SERVO_LEFT_PIN 5
#define SERVO_RIGHT_PIN 6

// --- サーボのニュートラルポイント ---
const int SERVO_LEFT_NEUTRAL = 1461;  // マイクロ秒
const int SERVO_RIGHT_NEUTRAL = 1438; // マイクロ秒

// --- MPU6050のオフセット値 ---
const int16_t GYRO_X_OFFSET = -704;
const int16_t GYRO_Y_OFFSET = -301;
const int16_t GYRO_Z_OFFSET = -130;

// --- PIDパラメータ ---
float Kp = 20.0;  // 比例ゲイン
float Ki = 0.1;   // 積分ゲイン
float Kd = 0.5;   // 微分ゲイン

// --- 相補フィルタ ---
const float alpha = 0.98; // ジャイロ積分の重み
float filteredAngle = 0.0;

// --- 制御の制限値 ---
const int MAX_OUTPUT = 500;

// --- 変数 ---
MPU6050 mpu;
Servo servoLeft, servoRight;

float previousError = 0.0;
float integral = 0.0;

unsigned long lastTime = 0;

// --- セットアップ ---
void setup() {
  Serial.begin(115200);
  Wire.begin();

  // MPU6050を初期化
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050接続失敗！");
    while (1);
  }

  // DLPFを6に設定（帯域幅 約5Hz）
  mpu.setDLPFMode(6);

  // サーボを初期化
  servoLeft.attach(SERVO_LEFT_PIN);
  servoRight.attach(SERVO_RIGHT_PIN);

  // 超音波センサを初期化
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  lastTime = millis();
}

// --- ループ処理 ---
void loop() {
  // MPU6050のデータを読み取る
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // ジャイロのオフセットを適用
  gx -= GYRO_X_OFFSET;
  gy -= GYRO_Y_OFFSET;
  gz -= GYRO_Z_OFFSET;

  // 加速度センサから傾斜角度を計算
  float accelAngle = atan2((float)ay, (float)az) * 180.0 / PI;

  // 時間差（Δt）
  unsigned long currentTime = millis();
  float dt = (currentTime - lastTime) / 1000.0;
  lastTime = currentTime;

  // ジャイロの回転速度（度/秒に変換）
  float gyroRate = gx / 131.0; // MPU6050感度: 131 LSB/(deg/s)

  // 相補フィルタ処理
  filteredAngle = alpha * (filteredAngle + gyroRate * dt) + (1 - alpha) * accelAngle;

  // 超音波距離を取得
  long distance = getDistance();

  // 目標角度を決定
  float targetAngle = 0.0; // デフォルトは0°
  if (distance < 5) {
    targetAngle = 5.0; // 距離が5cm未満なら目標角度を5°に設定
  }

  // 傾斜が±60°を超えたらモーターを停止
  if (abs(filteredAngle) > 60) {
    stopMotors();
    Serial.println("傾斜が60°を超えたため、モーターを停止しました。");
    return;
  }

  // PID制御
  float error = targetAngle - filteredAngle; // 目標角度を使用
  integral += error * dt;

  // integralが200を超えたらリセット
  if (abs(integral) > 200) {
    integral = 0.0;
    Serial.println("Integralが200を超えたためリセットしました。");
  }

  float derivative = (error - previousError) / dt;
  previousError = error;

  float output = Kp * error + Ki * integral + Kd * derivative;

  // 出力を±500に制限
  if (output > MAX_OUTPUT) output = MAX_OUTPUT;
  if (output < -MAX_OUTPUT) output = -MAX_OUTPUT;

  // モーターを制御
  controlMotors(output);

  // デバッグ情報
  Serial.print("Distance: "); Serial.print(distance);
  Serial.print(" cm | AccelAngle: "); Serial.print(accelAngle);
  Serial.print(" | FilteredAngle: "); Serial.print(filteredAngle);
  Serial.print(" | TargetAngle: "); Serial.print(targetAngle);
  Serial.print(" | Integral: "); Serial.print(integral);
  Serial.print(" | Output: "); Serial.println(output);
}

// --- モーター制御 ---
void controlMotors(float output) {
  int leftSpeed = SERVO_LEFT_NEUTRAL - (int)output;
  int rightSpeed = SERVO_RIGHT_NEUTRAL + (int)output;

  servoLeft.writeMicroseconds(leftSpeed);
  servoRight.writeMicroseconds(rightSpeed);
}

// --- モーター停止 ---
void stopMotors() {
  servoLeft.writeMicroseconds(SERVO_LEFT_NEUTRAL);
  servoRight.writeMicroseconds(SERVO_RIGHT_NEUTRAL);
}

// --- 超音波距離測定 ---
long getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  long distance = duration * 0.034 / 2;
  return distance;
}