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.

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

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));
  }
}

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);
  }
}