Self balancing robot PID

I have built a self-balancing robot, and despite numerous trials and errors in finding the right PID constants, I am feeling lost as the robot is not achieving stability. I have attached my code. Could you please review it and provide feedback on what I might have done wrong?

float calculateMotorSpeed(double pitch,double setpoint) {
  
  error = setpoint - pitch;
  double Pout = Kp * error;
  error_sum += error;
  double Iout = Ki * error_sum;
  double Dout = Kd * (error - last_error);
  float output = Pout + Iout + Dout;

  //output=0.7 *output;
  if (output > motor_speed_max) {
    output = motor_speed_max;
  }
   if (output < motor_speed_min) {
    output = motor_speed_min;
  }
  last_error = error;
  
  Serial.print("error: ");
  Serial.print(error);
  Serial.print(", output: ");
  Serial.println(output);

  return output;
}

out of curiosity, doesn't the code need 2 separate PID controllers for both front/back and side-to-side?

if its a robot like this it will not need side to side.

1 Like

no. it wasn't clear what you meant by self-balancing

how do you determine the setpoint -- presumably the upright position -- and how to you measure the pitch?

accelerometer?

yes it looks like this

1 Like

Where's the rest of the code? In what way is it unstable? Measurements? Data? What are the right constants? and what happend with the wrong constants? What's motor_speed_min/max?

There is more to PID than just this routine -- Depending on how frequently the routine is called, it affects the way the time-based PID elements (Ki & Kd) work. Any delays() would attenuate Ki and accentuate kD.

For experimenting I'd rewrite:

as

  Serial.print("ms:");
  Serial.print(millis()); // maybe mills()%100 to constrain for  serialplotter  
  Serial.print(" pitch:");
  Serial.print(pitch);
  Serial.print(" error:");
  Serial.print(error);
  Serial.print(" error_sum:");
  Serial.print(error_sum);
  Serial.print(" output:");
  Serial.println(output);

to see the time and if/how error_sum contributes to the instability. And maybe print before the clamping--If it happens to get saturated past motor_speed_min/max, it will be very hard to diagnose.

The "ms:55 pitch:1.23 error:12.34 error_sum:4.56 output:-78.90" format will be decoded by the serial plotter to label the traces with their names.

/* Get tilt angles on X and Y, and rotation angle on Z
 * Angles are given in degrees
 * 
 * License: MIT
 */

#include "Wire.h"
#include <MPU6050_light.h>

MPU6050 mpu(Wire);
unsigned long timer = 0;

const int in1Pin = 7;
const int in2Pin = 6;
const int in3Pin = 8;
const int in4Pin = 5;
const int motor1speed=9;
const int motor2speed=10;

double Iout=0;

double Kp = 10.5;
double Ki = 1;
double Kd = 25;

// Define the PID variables
double setpoint = 0;
double pitch = 0;
double roll = 0;
double error = 0;
double last_error = 0;
double error_sum = 0;

// Define the motor speed limits
int motor_speed_min = -180;
int motor_speed_max = 180;

unsigned long previousTime;
void setup() {
  Serial.begin(9600);
  Wire.begin();
  sensor_setup();
  motorSetup();
  //delay(50);
}
void sensor_setup()
{
   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(F("Calculating offsets, do not move MPU6050"));
  delay(200);
  // mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
  mpu.calcOffsets(); // gyro and accelero
 
  Serial.println("Done!\n");
}
void loop() {
   mpu.update();
   balanceRobot();
   //moveForward(100);
   //delay(1);
  
}
float getPitch()
{
  return mpu.getAngleX();
}
float getRoll()
{
  return mpu.getAngleY();
}
float getYaw()
{
  return mpu.getAngleZ();
}
void motorSetup()
{
 digitalWrite(in1Pin, 0);
 digitalWrite(in2Pin, 0);
 digitalWrite(in3Pin, 0);
 digitalWrite(in4Pin, 0);
 digitalWrite(motor1speed, 0);
 digitalWrite(motor2speed, 0);
 pinMode (in1Pin, OUTPUT);
 pinMode (in2Pin, OUTPUT);
 pinMode (in3Pin, OUTPUT);
 pinMode (in4Pin, OUTPUT); 
 pinMode(motor1speed, OUTPUT);
 pinMode(motor2speed, OUTPUT);
}
//in1,in2 pt motor dreapta cand intrerupatorul e in partea de sus
void moveForward(int speed)
{
   analogWrite(motor1speed, speed); // Set motor speed to 200 (out of 255)
    analogWrite(motor2speed, speed); // Set motor speed to 200 (out of 255)
  digitalWrite(in1Pin, HIGH); // Set motor1Pin1 to HIGH
  digitalWrite(in2Pin, LOW); // Set motor1Pin2 to LOW
  digitalWrite(in3Pin, HIGH); // Set motor1Pin1 to HIGH
  digitalWrite(in4Pin, LOW); // Set motor1Pin2 to LOW
  
}
void moveBackward(int speed)
{
  analogWrite(motor1speed, speed); // Set motor speed to 150 (out of 255)
  analogWrite(motor2speed, speed); // Set motor speed to 150 (out of 255)
  digitalWrite(in1Pin, LOW); // Set motor1Pin1 to LOW
  digitalWrite(in2Pin, HIGH); // Set motor1Pin2 to HIGH
  digitalWrite(in3Pin, LOW); // Set motor2Pin1 to LOW
  digitalWrite(in4Pin, HIGH); // Set motor2Pin2 to HIGH
}

float calculateMotorSpeed(double pitch,double setpoint) {
  
  error = setpoint - pitch;
  double Pout = Kp * error;
  //error_sum += error; //i've removed this part because the output increases too much ( >1000)
  Iout=Ki* error; // before was Iout= Ki*error_sum;
  double Dout = Kd * (error - last_error);
  float output = Pout + Iout + Dout;

//  if (output > motor_speed_max) {
//    output = motor_speed_max;
//  }
//   if (output < motor_speed_min) {
//    output = motor_speed_min;
//  }
  last_error = error;

  Serial.print("error: ");
  Serial.print(error);
  Serial.print(", output: ");
  Serial.println(output);

  return output;
}
void balanceRobot() {
  // Read pitch and roll sensors
  pitch = getPitch();
  
  // Set the setpoint based on the desired pitch angle
  setpoint = 0.5; // i've compute the average pitch angles when the robot was placed horizontally;

  // Calculate the motor speed using the PID controller
  float motor_speed = calculateMotorSpeed(pitch,setpoint);
  Serial.println(motor_speed);
  // Move the robot forward or backward based on the motor speed
  if (motor_speed > 0) {
    moveForward(motor_speed);
  }
  else if (motor_speed < 0) {
    moveBackward(abs(motor_speed));
  }
}
1 Like

i've modified Iout;
if i have Iout smth like this Iout=Iout+Ki*error the output increases a lot, even if the error is very low,
the output is above 1000.

It is very easy for Iout=Iout+Ki*error integral element to runaway if it is called 100x per second. I sometimes like using "conditional integration" to shut integration down if it is at the limits and help prevent saturation. Maybe:

 Iout+=Ki* error; // before was Iout= Ki*error_sum;
 double Dout = Kd * (error - last_error);
 float output = Pout + Iout + Dout;

 if (output > motor_speed_max) {
   Iout -= Ki* error;  // undo integration
   output = motor_speed_max;
 }
 if (output < motor_speed_min) {
   Iout -= Ki* error;  // undo integration
   output = motor_speed_min;
 }

**** Wait what? That breaks Ki from being an integral term to just being a proportional term. Might as well zero out Ki and add its weight to Kp. I modified the above to still be an integral.

i've computed the average value for setpoint when the robot is placed horizonally and i use also a level bubble. To measure the pitch angle i use already build in function which is available in the MPU6050 library.

did you manage to complete the project? I am also very close to the solution but struggling to tune the PD loop. I also recommend to implement a complementary filter for your code so you can combine both the gyro and the accelerometer readings. The default functions are not doing it. ill post my code below which I am trying to improve:

//Include I2C library and declare variables
#include <Wire.h>
#include <MPU6050.h>

long loopTimer, loopTimer2;
int temperature;
double accelPitch;
double accelRoll;
long acc_x, acc_y, acc_z;
double accel_x, accel_y, accel_z;
double gyroRoll, gyroPitch, gyroYaw;
int gyro_x, gyro_y, gyro_z;
//long gyro_x_cal, gyro_y_cal, gyro_z_cal, acc_x_cal, acc_y_cal, acc_z_cal;  //dont need this after hard coding the offset
long gyro_x_cal = 181, gyro_y_cal = -655, gyro_z_cal = -39 , acc_x_cal, acc_y_cal, acc_z_cal;
double rotation_x, rotation_y, rotation_z;
double freq, dt;
double tau = 0.98;
double roll = 0;
double pitch = 0;

// 250 deg/s --> 131.0, 500 deg/s --> 65.5, 1000 deg/s --> 32.8, 2000 deg/s --> 16.4
long scaleFactorGyro = 65.5;

// 2g --> 16384 , 4g --> 8192 , 8g --> 4096, 16g --> 2048
long scaleFactorAccel = 8192;


//pins for the motor driver

#define M1_d 2
#define M1_dr 3
#define M1_speed 9
#define M2_d 4
#define M2_dr 5
#define M2_speed 10
#define min_speed 33


//Bunch of variables
int pwm;
float err, Kp = 14.0, Kd = 0; //this sets the sensitivity of the motors in regards to tilt. Tune it with trial and error
bool fwd = false;  //decide to drive motor fwd or bkwd
float errd;  //variable to store derivative of error
float fltr; //use this to smooth out rough data
double Ox=0;  //store previous reading for next cycle
float set_point_offset;  //allowing set point calibration

#define P_pot A0           //pot to set the Kp value 0 to 30
#define D_pot A1           //pot to set the Kd value 0 to 20
#define set_point_offset_pot A2   // use this pot to change the centre set point

#define BALANCE_LED 12    //LED that lights up when robot is balanced

void setup() {
  // Start
  Wire.begin();
  Serial.begin(115200);

  // Setup the registers of the MPU-6050 and start up
  setup_mpu_6050_registers();

  //calibrate_sensor();   //dont need this anymore

  // Display headers
  /*Serial.print("\nNote 1: Yaw is not filtered and will drift!\n");
  Serial.print("\nNote 2: Make sure sampling frequency is ~250 Hz\n");
  Serial.print("Sampling Frequency (Hz)\t\t");
  Serial.print("Roll (deg)\t\t");
  Serial.print("Pitch (deg)\t\t");
  Serial.print("Yaw (deg)\t\t\n");*/

  //set motor driver pin as output
  pinMode(M1_d , OUTPUT);
  pinMode(M1_dr , OUTPUT);
  pinMode(M1_speed , OUTPUT);
  pinMode(M2_d , OUTPUT);
  pinMode(M2_dr , OUTPUT);
  pinMode(M2_speed , OUTPUT);
  pinMode(P_pot , INPUT);
  pinMode(D_pot , INPUT);
  pinMode(set_point_offset_pot , INPUT);

  // make surre the motors are OFF when powered up
  digitalWrite(M1_d , LOW);
  digitalWrite(M1_dr , LOW);
  digitalWrite(M1_speed , LOW);
  digitalWrite(M2_d , LOW);
  digitalWrite(M2_dr , LOW);
  digitalWrite(M2_speed , LOW);

  //set teh LED as output
  pinMode(BALANCE_LED, OUTPUT); 

  // Reset the loop timer
  loopTimer = micros();
  loopTimer2 = micros();
}


void loop() {
  freq = 1/((micros() - loopTimer2) * 1e-6);
  loopTimer2 = micros();
  dt = 1/freq;

  // Read the raw acc data from MPU-6050
  read_mpu_6050_data();

  // Subtract the offset calibration value
  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;

  // Convert to instantaneous degrees per second
  rotation_x = (double)gyro_x / (double)scaleFactorGyro;
  rotation_y = (double)gyro_y / (double)scaleFactorGyro;
  rotation_z = (double)gyro_z / (double)scaleFactorGyro;

  // Convert to g force
  accel_x = (double)acc_x / (double)scaleFactorAccel;
  accel_y = (double)acc_y / (double)scaleFactorAccel;
  accel_z = (double)acc_z / (double)scaleFactorAccel;

  // Complementary filter
  accelPitch = atan2(accel_y, accel_z) * RAD_TO_DEG;
  accelRoll = atan2(accel_x, accel_z) * RAD_TO_DEG;

  pitch = (tau)*(pitch + rotation_x*dt) + (1-tau)*(accelPitch);
  roll = (tau)*(roll - rotation_y*dt) + (1-tau)*(accelRoll);

  gyroPitch += rotation_x*dt;
  gyroRoll -= rotation_y*dt;
  gyroYaw += rotation_z*dt;

  // Visualize just the roll
  // Serial.print(roll); Serial.print(",");
  // Serial.print(gyroRoll); Serial.print(",");
  // Serial.println(accelRoll);

  // Visualize just the pitch
  // Serial.print(pitch); Serial.print(",");
  // Serial.print(gyroPitch); Serial.print(",");
  // Serial.println(accelPitch);

  // Data out serial monitor
   Serial.print(freq,0);   Serial.print(",");
   Serial.print(roll - set_point_offset,1);   Serial.print(", \t");
   Serial.print(" Kp = ");    Serial.print(Kp);
   Serial.print(" Kd = ");    Serial.println(Kd);     

  
  set_PD_constants();   //reading pots to calibrate PD values
  calibrate_set_point_offset(); //read pot to adjust centre point
  balance_LED();         //light up an LED if robot is balanced

  // Wait until the loopTimer reaches 4000us (250Hz) before next loop
  while (micros() - loopTimer <= 4000);
  loopTimer = micros();

}

void calibrate_set_point_offset(){
  set_point_offset = analogRead(set_point_offset_pot);
  set_point_offset = (float(set_point_offset / 1023) * 10) - 5 ; //the value goes from +- 5 degrees

}

void set_PD_constants(){
  Kp = analogRead(P_pot); //read Kp pot
  Kd = analogRead(D_pot); //read Kd pot
  Kp = (Kp / 1023) * 30;  //map the values
  Kd = (Kd / 1023) * 20;
}

void balance_LED(){
  if ((roll - set_point_offset) > -0.3 && (roll - set_point_offset) < 0.3){ //if almost balanced with 0.3 degrees of tollerance
    digitalWrite(BALANCE_LED ,1);
  }
  else{
    digitalWrite(BALANCE_LED ,0);
    // moved here because I dont wanna update motors if no need
    balance_robot();      //update the pwm and direction
  }
}

void read_mpu_6050_data() {
  // Subroutine for reading the raw data
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 14);

  // Read data --> Temperature falls between acc and gyro registers
  acc_x = Wire.read() << 8 | Wire.read();
  acc_y = Wire.read() << 8 | Wire.read();
  acc_z = Wire.read() << 8 | Wire.read();
  temperature = Wire.read() <<8 | Wire.read();
  gyro_x = Wire.read()<<8 | Wire.read();
  gyro_y = Wire.read()<<8 | Wire.read();
  gyro_z = Wire.read()<<8 | Wire.read();
}

void setup_mpu_6050_registers() {
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  // Configure the accelerometer
  // Wire.write(0x__);
  // Wire.write; 2g --> 0x00, 4g --> 0x08, 8g --> 0x10, 16g --> 0x18
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x08);
  Wire.endTransmission();
  // Configure the gyro
  // Wire.write(0x__);
  // 250 deg/s --> 0x00, 500 deg/s --> 0x08, 1000 deg/s --> 0x10, 2000 deg/s --> 0x18
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();
}

void calibrate_sensor(){
  int readings_num = 1000;  //1000 samples
  // Calibration
  Serial.println("Calibrating sensor, place on level surface and do not move.");

  // Take  readings for each coordinate and then find average offset
  for (int cal_int = 0; cal_int < readings_num; cal_int ++){
    if(cal_int % 200 == 0)Serial.print(".");  //print . every 200 cycles
    read_mpu_6050_data();
    gyro_x_cal += gyro_x;
    gyro_y_cal += gyro_y;
    gyro_z_cal += gyro_z;

    acc_x_cal += acc_x;
    acc_y_cal += acc_y;
    acc_z_cal += acc_z;
    delay(3);
  }
  // Average the values
  gyro_x_cal /= readings_num;
  gyro_y_cal /= readings_num;
  gyro_z_cal /= readings_num;

  acc_x_cal /= readings_num;
  acc_y_cal /= readings_num;
  acc_z_cal /= readings_num;  
}


void balance_robot(){
  err = float(roll) - set_point_offset;   //error from setpoint
  errd = err - Ox;                        //error for Kd
  Ox = err;                               //preparing for next cycle 

  if (err > 0){   //if tilted fw/bkw
    fwd = true;
  }
  else{
    fwd = false;
  }
  err = abs(err); //dont want to feed a negative pwm value
  //Serial.println(err);  //debug line
  //Serial.println(set_point_offset);  //debug line
  //set motor speed
  pwm = err * Kp + errd * Kd;  //value with PD loop
  pwm = constrain(pwm, min_speed , 255);  //make sure value fits input parameter

  analogWrite(M1_speed , pwm);  //set motor speed 
  analogWrite(M2_speed + 5, pwm);  //add 5 because this motor is a bit weaker

  if (fwd){   //if tilted fw/bkw
    digitalWrite(M1_d , HIGH);
    digitalWrite(M1_dr , LOW);
    digitalWrite(M2_d , HIGH);
    digitalWrite(M2_dr , LOW);
  }
  else{
    digitalWrite(M1_d , LOW);
    digitalWrite(M1_dr , HIGH);
    digitalWrite(M2_d , LOW);
    digitalWrite(M2_dr , HIGH);
  }
}

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.