موتورسیکلت خودمتوازن DIY — بخش ۲: MPU-6050، PID آردوینو و درایور موتور (فارسی)

BOM الکترونیک

سیم‌کشی

کد کامل PID دوگانه برای موتورسیکلت خودمتوازن

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

MPU6050 mpu;
Servo steeringServo;

// ===== پارامترهای PID تعادل =====
float Kp_bal = 35.0, Ki_bal = 0.8, Kd_bal = 2.0;
// ===== پارامترهای PID فرمان =====
float Kp_str = 3.0, Ki_str = 0.0, Kd_str = 0.5;

float setpointAngle = 1.5;  // زاویه تعادل (کمی رو به جلو)
float setpointYaw   = 0.0;

float angle = 0.0, yawAngle = 0.0;
float errBal=0, sumBal=0, lastErrBal=0;
float errStr=0, sumStr=0, lastErrStr=0;
unsigned long lastTime;

// پین‌های موتور
#define IN1 7
#define IN2 8
#define ENA 9
#define SERVO_PIN 10

void setMotor(int speed) {
  speed = constrain(speed, -255, 255);
  if (speed >= 0) { digitalWrite(IN1,HIGH); digitalWrite(IN2,LOW); }
  else            { digitalWrite(IN1,LOW);  digitalWrite(IN2,HIGH); speed = -speed; }
  analogWrite(ENA, speed);
}

void setup() {
  pinMode(IN1,OUTPUT); pinMode(IN2,OUTPUT); pinMode(ENA,OUTPUT);
  steeringServo.attach(SERVO_PIN);
  steeringServo.write(90);   // وسط
  Wire.begin();
  mpu.initialize();
  lastTime = millis();
}

void loop() {
  int16_t ax,ay,az,gx,gy,gz;
  mpu.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);

  float dt = (millis() - lastTime) / 1000.0;
  lastTime = millis();

  // فیلتر مکمل — pitch
  float accelAngle = atan2(ay, az) * 180.0 / PI;
  angle = 0.98*(angle + (gx/131.0)*dt) + 0.02*accelAngle;

  // yaw تجمیعی از ژیرو Z
  yawAngle += (gz/131.0)*dt;

  // ===== PID تعادل =====
  errBal = setpointAngle - angle;
  sumBal += errBal*dt; sumBal = constrain(sumBal,-100,100);
  float outBal = Kp_bal*errBal + Ki_bal*sumBal + Kd_bal*(errBal-lastErrBal)/dt;
  outBal = constrain(outBal,-255,255);
  lastErrBal = errBal;
  setMotor((int)outBal);

  // ===== PID فرمان =====
  errStr = setpointYaw - yawAngle;
  sumStr += errStr*dt; sumStr = constrain(sumStr,-30,30);
  float outStr = Kp_str*errStr + Ki_str*sumStr + Kd_str*(errStr-lastErrStr)/dt;
  outStr = constrain(outStr,-45,45);
  lastErrStr = errStr;
  steeringServo.write(90 + (int)outStr);

  delay(10);
}

تنظیم و آزمایش

1. ابتدا موتور عقب را با دست بلند نگه دار — روی هوا تنظیم Kp_bal کن تا موتور به شیب واکنش درست نشان دهد.
2. سرو را جدا تست کن: چک کن ۹۰° = مستقیم است.
3. اول Ki و Kd را صفر کن — فقط با Kp تعادل اولیه بگیر.
4. Kd را اضافه کن تا نوسان کم شود. سپس Ki کمی اضافه کن.
5. setpointAngle را تنظیم کن تا موتورسیکلت در جای خود بایستد بدون حرکت به جلو یا عقب.

• کتابخانه MPU-6050 — GitHub
• آپارات: ربات خودمتوازن فارسی