ربات تک‌چرخ (مونوویل) تعادلی با آردوینو و پرینتر سه‌بعدی

#include <Wire.h>
#include <Servo.h>
#define MPU_ADDR 0x68
#define RAD2DEG 57.29577951308232f
#define GYRO_SCALE 131.0f
const int SERVO_PIN = 5;
const int EN1 = 9;
const int IN1 = 7;
const int IN2 = 6;
const int IR_PIN = 2;
int SERVO_CENTER = 110;
const int SERVO_MIN = 70;
const int SERVO_MAX = 160;
float SERVO_SLEW = 3.0f;
const float CF_ALPHA = 0.995f;
float filterAngle = 0.0f;
float accelZero = 0.0f;
float Kp = 8.0f;
float Ki = 0.6f;
float Kd = 1.2f;
float integral = 0.0f;
float lastError = 0.0f;
float lastDerivative = 0.0f;
const float I_LIMIT = 200.0f;
const float DERIV_FILTER_ALPHA = 0.7f;
float servoPosSmooth = SERVO_CENTER;
int ACC_AXIS = 1;
int GYRO_AXIS = 1;
int SIGN_ACC = 1;
int SIGN_GYRO = 1;
int SIGN_Z = 1;
Servo steer;
unsigned long lastMicros = 0;
float gyroBias[3] = {0.0f,0.0f,0.0f};
volatile bool irPulseFlag = false;
unsigned long lastIrToggleMs = 0;
const unsigned long IR_DEBOUNCE_MS = 500;
int motorPWM = 200;
bool motorState = false;
// -------- Function Prototypes --------
void setMotor(bool on);
void irISR();
void handleSerial();
void printStatus();
void mpureg_write(uint8_t reg, uint8_t val);
void readAccelGyro(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz);
float readAccelAngleRaw();
void calibrateGyroBias();
// -------- Setup --------
void setup(){
Serial.begin(115200);
Wire.begin();
pinMode(EN1, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
analogWrite(EN1, 0);
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
pinMode(IR_PIN, INPUT);
attachInterrupt(digitalPinToInterrupt(IR_PIN), irISR, CHANGE);
steer.attach(SERVO_PIN);
steer.write(SERVO_CENTER);
servoPosSmooth = SERVO_CENTER;
mpureg_write(0x6B, 0x00);
delay(100);
mpureg_write(0x1B, 0x00);
delay(50);
Serial.println("\n=== BALANCE PID START ===");
Serial.println("Hold board upright and still during calibration...");
delay(1500);
calibrateGyroBias();
accelZero = readAccelAngleRaw();
filterAngle = 0.0f;
lastMicros = micros();
Serial.println("Calibration done. Commands: on/off kp/ki/kd center invacc invgyro invz s");
printStatus();
}
// -------- Loop --------
void loop(){
handleSerial();
if(irPulseFlag){
irPulseFlag = false;
if(millis() - lastIrToggleMs > IR_DEBOUNCE_MS){
lastIrToggleMs = millis();
setMotor(!motorState);
}
}
unsigned long now = micros();
float dt = (now - lastMicros) / 1e6f;
if(dt <= 0) dt = 0.001f;
lastMicros = now;
int16_t ax, ay, az, gx, gy, gz;
readAccelGyro(ax, ay, az, gx, gy, gz);
float acc[3] = {(float)ax, (float)ay, (float)az};
float g[3] = {(float)gx, (float)gy, (float)gz};
float accLat = SIGN_ACC * acc[ACC_AXIS];
float accVert = SIGN_Z * acc[2];
float accAngle = atan2(accLat, accVert) * RAD2DEG;
float gyroRate = SIGN_GYRO * ((g[GYRO_AXIS] - gyroBias[GYRO_AXIS]) / GYRO_SCALE);
filterAngle = CF_ALPHA * (filterAngle + gyroRate * dt) + (1.0f - CF_ALPHA) * accAngle;
float setpoint = 0.0f;
float error = setpoint - filterAngle;
integral += error * dt;
if(integral > I_LIMIT) integral = I_LIMIT;
if(integral < -I_LIMIT) integral = -I_LIMIT;
float derivativeRaw = (error - lastError) / dt;
float derivative = DERIV_FILTER_ALPHA * lastDerivative + (1.0f - DERIV_FILTER_ALPHA) * derivativeRaw;
lastDerivative = derivative;
float output = Kp * error + Ki * integral + Kd * derivative;
lastError = error;
float targetServo = SERVO_CENTER - output;
if(targetServo < SERVO_MIN) targetServo = SERVO_MIN;
if(targetServo > SERVO_MAX) targetServo = SERVO_MAX;
if(targetServo > servoPosSmooth + SERVO_SLEW) servoPosSmooth += SERVO_SLEW;
else if(targetServo < servoPosSmooth - SERVO_SLEW) servoPosSmooth -= SERVO_SLEW;
else servoPosSmooth = targetServo;
steer.write((int)servoPosSmooth);
static unsigned long lastPrint = 0;
if(millis() - lastPrint > 200){
Serial.print("angle=");
Serial.print(filterAngle, 2);
Serial.print(" err=");
Serial.print(error, 3);
Serial.print(" out=");
Serial.print(output, 3);
Serial.print(" servo=");
Serial.print((int)servoPosSmooth);
Serial.print(" motor=");
Serial.println(motorState ? "ON" : "OFF");
lastPrint = millis();
}
delay(6);
}
// -------- Motor --------
void setMotor(bool on){
if(on){
analogWrite(EN1, motorPWM);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
motorState = true;
Serial.println("MOTOR ON");
} else {
analogWrite(EN1, 0);
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
motorState = false;
Serial.println("MOTOR OFF");
}
}
// -------- IR --------
void irISR(){
irPulseFlag = true;
}
// -------- Serial --------
void handleSerial(){
while(Serial.available()){
String s = Serial.readStringUntil('\n');
s.trim();
if(s.length() == 0) return;
if(s.equalsIgnoreCase("on")) setMotor(true);
else if(s.equalsIgnoreCase("off")) setMotor(false);
else if(s.startsWith("kp ")) { Kp = s.substring(3).toFloat(); Serial.print("Kp="); Serial.println(Kp); }
else if(s.startsWith("ki ")) { Ki = s.substring(3).toFloat(); Serial.print("Ki="); Serial.println(Ki); }
else if(s.startsWith("kd ")) { Kd = s.substring(3).toFloat(); Serial.print("Kd="); Serial.println(Kd); }
else if(s.equalsIgnoreCase("center")) { SERVO_CENTER = (int)servoPosSmooth; Serial.print("Set SERVO_CENTER="); Serial.println(SERVO_CENTER); }
else if(s.equalsIgnoreCase("invacc")) { SIGN_ACC = -SIGN_ACC; Serial.print("SIGN_ACC="); Serial.println(SIGN_ACC); }
else if(s.equalsIgnoreCase("invgyro")) { SIGN_GYRO = -SIGN_GYRO; Serial.print("SIGN_GYRO="); Serial.println(SIGN_GYRO); }
else if(s.equalsIgnoreCase("invz")) { SIGN_Z = -SIGN_Z; Serial.print("SIGN_Z="); Serial.println(SIGN_Z); }
else if(s.equalsIgnoreCase("s")) printStatus();
else { Serial.print("Unknown cmd: "); Serial.println(s); }
}
}
// -------- Status --------
void printStatus(){
Serial.println("--- STATUS ---");
Serial.print("Kp="); Serial.print(Kp);
Serial.print(" Ki="); Serial.print(Ki);
Serial.print(" Kd="); Serial.println(Kd);
Serial.print("ACC_AXIS="); Serial.print(ACC_AXIS);
Serial.print(" SIGN_ACC="); Serial.println(SIGN_ACC);
Serial.print("GYRO_AXIS="); Serial.print(GYRO_AXIS);
Serial.print(" SIGN_GYRO="); Serial.println(SIGN_GYRO);
Serial.print("SIGN_Z="); Serial.println(SIGN_Z);
Serial.print("SERVO_CENTER="); Serial.println(SERVO_CENTER);
Serial.print("IR_PIN="); Serial.println(IR_PIN);
Serial.println("--------------");
}
// -------- MPU helpers --------
void mpureg_write(uint8_t reg, uint8_t val){
Wire.beginTransmission(MPU_ADDR);
Wire.write(reg);
Wire.write(val);
Wire.endTransmission();
}
void readAccelGyro(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz){
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDR, 14, true);
ax = (Wire.read() << 8) | Wire.read();
ay = (Wire.read() << 8) | Wire.read();
az = (Wire.read() << 8) | Wire.read();
Wire.read(); Wire.read();
gx = (Wire.read() << 8) | Wire.read();
gy = (Wire.read() << 8) | Wire.read();
gz = (Wire.read() << 8) | Wire.read();
}
float readAccelAngleRaw(){
int16_t ax, ay, az, gx, gy, gz;
readAccelGyro(ax, ay, az, gx, gy, gz);
float a[3] = {(float)ax, (float)ay, (float)az};
float lat = SIGN_ACC * a[ACC_AXIS];
float vert = SIGN_Z * a[2];
return atan2(lat, vert) * RAD2DEG;
}
void calibrateGyroBias(){
const int N = 400;
long sum[3] = {0,0,0};
Serial.println("Calibrating gyro bias — keep STILL...");
for(int i=0;i<N;i++){
int16_t ax,ay,az,gx,gy,gz;
readAccelGyro(ax,ay,az,gx,gy,gz);
sum[0]+=gx; sum[1]+=gy; sum[2]+=gz;
delay(3);
}
gyroBias[0] = (float)sum[0]/N;
gyroBias[1] = (float)sum[1]/N;
gyroBias[2] = (float)sum[2]/N;
Serial.print("gyroBias X,Y,Z = ");
Serial.print(gyroBias[0],2); Serial.print(" ");
Serial.print(gyroBias[1],2); Serial.print(" ");
Serial.println(gyroBias[2],2);
}