Hello everyone,
I’m working on a self-balancing robot using an MPU6050, DC motors, and encoders. I’ve implemented a PID-based control system with separate tilt (inner loop) and position (outer loop) controllers. The robot balances perfectly when tested on a flat, smooth surface like a bed. However, when placed on the floor, it starts drifting forward or backward instead of staying in place.
Here are the details of my setup:
- Hardware:
- MPU6050 for tilt angle measurement.
- Encoders for tracking wheel rotation and position.
- DC motors with PWM control.
- Software:
- The tilt PID stabilizes the robot’s angle (inner loop).
- The position PID corrects the robot's drift (outer loop).
- Gains for both loops have been tuned experimentally.
- Observations:
- The robot balances perfectly on the bed (no drift).
- When placed on the floor, it drifts forward or backward.
- Drifting gets worse when the surface is less smooth or has minor inclines.
Here is my code which i am working on
#include "Wire.h"
#include "MPU6050_light.h"
// Define MPU6050 object
MPU6050 mpu(Wire);
// Variables for tilt angle
float tilt = 0.0, tilt_old = 0.0, tilt_dot = 0.0, integral_tilt = 0.0;
// Variables for wheel angle
int wheel_pulse_count = 0;
float wheel_angle = 0.0, wheel_angle_old = 0.0, wheel_dot = 0.0, integral_wheel = 0.0;
// Final output for wheel velocity
int control_output = 0.0;
// Tilt PID gains
// float kp_tilt = 0.0; // Proportional gain for tilt
// float kd_tilt = 0.0; // Derivative gain for tilt
// float ki_tilt = 0.0; // Integral gain for tilt
float kp_tilt = 60.0; // Proportional gain for tilt
float kd_tilt = 0.03; // Derivative gain for tilt
float ki_tilt = 50.0; // Integral gain for tilt
// Position PID gains
// float kp_wheel = 0.0; // Proportional gain for wheel
// float kd_wheel = 0.0; // Derivative gain for wheel
// float ki_wheel = 0.0; // Integral gain for wheel
float kp_wheel = 5.0; // Proportional gain for wheel
float kd_wheel = 0.01; // Derivative gain for wheel
float ki_wheel = 0.0; // Integral gain for wheel
// Define motor pins
#define InL1 9 // INA motor pin
#define PWML 5 // PWM motor pin
#define InL2 4 // INB motor pin
#define InR1 A2 // INA motor pin
#define PWMR 6 // PWM motor pin
#define InR2 A3 // INB motor pin
// Define encoder pins
#define encodPinAR 2 // Encoder A pin
#define encodPinBR 7 // Encoder B pin
// Motor initialization function
void motor_init() {
// Left motor pins
pinMode(InL1, OUTPUT);
pinMode(InL2, OUTPUT);
pinMode(PWML, OUTPUT);
digitalWrite(InL1, LOW);
digitalWrite(InL2, LOW);
analogWrite(PWML, 0);
// Right motor pins
pinMode(InR1, OUTPUT);
pinMode(InR2, OUTPUT);
pinMode(PWMR, OUTPUT);
digitalWrite(InR1, LOW);
digitalWrite(InR2, LOW);
analogWrite(PWMR, 0);
}
// Timer1 ISR for IMU
void timer1_init() {
cli(); // Clear global interrupts
TIMSK1 = 0x01; // Timer5 overflow interrupt enable
TCCR1B = 0x00; // Stop
TCNT1H = 0xA2; // Counter higher 8 bit value
TCNT1L = 0x3F; // Counter lower 8 bit value
TCCR1A = 0x00;
TCCR1C = 0x00;
TCCR1B = 0x02; // Start Timer, prescaler 8
sei(); // Enable global interrupts
}
// Motor control function for left motor
void motor_control_L(int pwm) {
if (pwm < 0) {
digitalWrite(InL1, HIGH);
digitalWrite(InL2, LOW);
pwm = -pwm;
} else {
digitalWrite(InL1, LOW);
digitalWrite(InL2, HIGH);
}
analogWrite(PWML, pwm);
}
// Motor control function for right motor
void motor_control_R(int pwm) {
if (pwm < 0) {
digitalWrite(InR1, HIGH);
digitalWrite(InR2, LOW);
pwm = -pwm;
} else {
digitalWrite(InR1, LOW);
digitalWrite(InR2, HIGH);
}
analogWrite(PWMR, pwm);
}
// ISR for Timer1 overflow
ISR(TIMER1_OVF_vect) {
sei();
// Set timer value for 12ms overflow
TCNT1H = 0xA2;
TCNT1L = 0x3F;
mpu.update(); // I2C needs global interrupt to be enabled
cli();
feedback();
control_eqn();
}
// Feedback function
void feedback() {
// Get tilt angle from MPU6050
tilt = mpu.getAngleY(); // MPU gives tilt in degrees
float dt = 0.012; // Timer overflow interval in seconds
// Calculate tilt rate of change
tilt_dot = (tilt - tilt_old) / dt;
tilt_old = tilt;
// Calculate wheel angle from encoder counts
wheel_angle = (wheel_pulse_count * 360.0) / 700.0; // Assuming 700 pulses per wheel revolution
Serial.print(tilt);
Serial.print(" ");
Serial.print(wheel_angle);
Serial.print(" ");
// Calculate wheel angular velocity
wheel_dot = (wheel_angle - wheel_angle_old) / dt;
wheel_angle_old = wheel_angle;
}
// Control equation function
void control_eqn() {
float dt = 0.012; // Timer overflow interval
// Tilt PID
float tilt_error = tilt - 0.25 ; // Desired tilt is 0 (upright)
integral_tilt += tilt_error * dt;
float tilt_output = kp_tilt * tilt_error + ki_tilt * integral_tilt + kd_tilt * tilt_dot;
// Position PID
float desired_position = 0.0; // Default to maintaining position
float position_error = wheel_angle - desired_position;
integral_wheel += position_error * dt;
float position_output = kp_wheel * position_error + ki_wheel * integral_wheel + kd_wheel * wheel_dot;
// Combine PID outputs
control_output = tilt_output + position_output;
// Constrain the output to motor limits
control_output = constrain(control_output, -255, 255);
// Map control output to effective motor range
int min_pwm = 60; // Minimum PWM to overcome static friction
if (control_output > 0) {
control_output = map(control_output, 1, 255, min_pwm, 255);
} else if (control_output < 0) {
control_output = map(control_output, -1, -255, -min_pwm, -255);
} else {
control_output = 0; // Stop the motor if the output is exactly 0
}
Serial.println(control_output);
// Send control signal to motors
motor_control_R(control_output );
motor_control_L(-control_output);
}
// Setup function
void setup() {
Serial.begin(115200);
// MPU6050 setup
Wire.begin();
byte status = mpu.begin();
Serial.print(F("MPU6050 status: "));
Serial.println(status);
while (status != 0) {} // Stop everything if could not connect to MPU6050
Serial.println("MPU begin done!\n");
delay(1000); // Required for offset calculation of IMU
mpu.calcOffsets(true, true);
// Motor setup
Serial.println("Begin Device initialization:\n");
motor_init();
Serial.println("DC Motor initialized\n");
// Timer ISR setup
timer1_init();
Serial.println("Timer initialized\n");
// Encoder interrupt setup
attachInterrupt(digitalPinToInterrupt(2), motor_encoder, RISING);
}
// Loop function
void loop() {
// Add any debugging or external control logic here
}
// ISR for motor encoder
void motor_encoder() {
if (digitalRead(encodPinBR) == HIGH) {
wheel_pulse_count = wheel_pulse_count - 1;
} else {
wheel_pulse_count = wheel_pulse_count + 1;
}
}