Self Balancing Robot Help

Hello everyone! I am currently working in the code of my self balancing robot and I have some problems. The robot itself balances until it tilts too much to one side and when that happens the robot starts accelerating until it hits something. I do not know what is exactly the problem.
At the bottom of the post I attached a video of it
Code:


//////////////////////////////////////////////////////////////
//// status LED stuff
#define statusPin 13
//#define statusPin 2 // for ESP8266-12

// Flashes the led without using the delay() function.
// The period of each cycle is flashDelay milliseconds
// flashSpeed determines the speed of each flash
// flashTimes determines the number of times to flash in a cycle
// Need to call this function inside a loop in order to flash
// in order to notice a pause between each cycle
// flashDelay should be greater than flashSpeed*2*flashTimes
//  e.g.   flashActivityLed(1000, 100, 1);
void flashActivityLed(int flashDelay=3000, int flashSpeed=150, int flashTimes=2) {
  static unsigned long nextLedActivity = 0;
  static int t = 0;
  if ((millis() > nextLedActivity)) {
    if (t == 0) {
      digitalWrite(statusPin, HIGH); // turn on LED
    } else {
      digitalWrite(statusPin, !digitalRead(statusPin)); // toggle LED
    }
    if (t < flashTimes*2-1) {
      nextLedActivity = millis() + flashSpeed;
      t++;
    } else {
      nextLedActivity = millis() + flashDelay - (flashSpeed*2*flashTimes);
      t = 0;
    }
  }
}


//////////////////////////////////////////////////////////////
// PID stuff
// PID stands for proportional-integral-derivative
// A PID controller uses a feedback loop mechanism to control a system
// to maintain a steady state.


#include <PID_v1.h> 


double setpoint = 184; //set the value when the robot is perpendicular to ground using serial monitor. 
// this is good! works
//double Kp = 17; //Set this first
//double Kd = 0.7; //Set this secound
//double Ki = 120; //Finally set this 

// this is good! works
double Kp = 23; //Set this first
double Kd = 0.8; //Set this secound
double Ki = 140; //Finally set this 

/******End of values setting*********/

double pidInput, pidOutput;
PID pid(&pidInput, &pidOutput, &setpoint, Kp, Ki, Kd, DIRECT);

void initPID() {
  pid.SetMode(AUTOMATIC);
  pid.SetSampleTime(10);
  pid.SetOutputLimits(-255, 255);  
}

//////////////////////////////////////////////////////////////
// MPU6050 stuff



#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
//#define USE_INTERRUPT     // define this to use interrupts
                            // the INT pin must be connected if using interrupts
#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
//#define INTERRUPT_PIN 15 // use pin 15 on ESP8266
volatile bool mpuInterrupted = false;     // indicates whether MPU interrupt pin has gone high

//// Interrupt detection routine
// When the MPU has data, it will interrupt the Arduino causing it
// to execute this routine automatically
void dmpDataReady() {
    mpuInterrupted = true;
}

//// initialize and calibrate the MPU
//// return true if successful
bool initMPU() {
  uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  // initialize device
  Serial.println("Initializing I2C devices...");
  mpu.initialize();
  pinMode(INTERRUPT_PIN, pidInput);

  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  // load and configure the DMP
  Serial.println("Initializing DMP...");
  devStatus = mpu.dmpInitialize();

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    Serial.print("Calibrating DMP...");
    // Don't need to calibrate the zero values.
    // We always want the robot to be upright at the setpoint value
    // So it is important to set the setpoint value for when the robot is upright
//    mpu.CalibrateAccel(6);
//    mpu.CalibrateGyro(6);
//    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    Serial.println("Enabling DMP...");
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.print("Enabling interrupt detection (Arduino external interrupt #");
    Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
    Serial.print(" on pin ");
    Serial.print(INTERRUPT_PIN);
    Serial.println(")...");
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println("DMP ready! Waiting for first interrupt...");
    return true;

  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print("DMP Initialization failed (code ");
    Serial.print(devStatus);
    Serial.println(")");
    while(1); // stop here if init failed
    return false;
  }
}


// Check if there's data from the MPU
// and if there's data then
// get and return the gyro x, y, z axis data
// This routine must be called very often
// Otherwise the FIFO will overflow
void checkMPU(float xyz[3]) {
  // MPU control/status vars
  static uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
  static uint16_t fifoCount;     // count of all bytes currently in FIFO
  static uint8_t fifoBuffer[64]; // FIFO storage buffer
  static uint16_t packetSize = mpu.dmpGetFIFOPacketSize();    // expected DMP packet size for later comparison (default is 42 bytes)
  // orientation/motion vars
  static Quaternion q;           // [w, x, y, z]         quaternion container
  static VectorFloat gravity;    // [x, y, z]            gravity vector
  static float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

#ifdef USE_INTERRUPT
  // check if there's data in the fifo
  if (!mpuInterrupted && fifoCount < packetSize) {
    return; // no data yet, so return
  }
#endif

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupted = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();
  
  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();
      //  fifoCount = mpu.getFIFOCount();  // will be zero after reset no need to ask
      Serial.println("FIFO overflow!");
  } 
  
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // get and return the gyro x, y, z axis data
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    xyz[0] = ypr[2] * 180/M_PI;   // roll(x axis)
    xyz[1] = ypr[1] * 180/M_PI;   // pitch(y axis)
    xyz[2] = ypr[0] * 180/M_PI;   // yaw(z axis)
  }
}

// for testing only
void testGyro() {
  static float xyz[3];
  do {
    checkMPU(xyz);
    Serial.print("x = ");
    Serial.print(xyz[0]);
    Serial.print("  y = ");
    Serial.print(xyz[1]);
    Serial.print("  z = ");
    Serial.print(xyz[2]);
    
    pidInput = xyz[0] + 180;    // get and adjust X-axis value
    pid.Compute();  // compute the new PID correction value
    Serial.print("    ");
    Serial.print(pidInput); Serial.print(" => "); Serial.print(pidOutput);
    Serial.println();
  } while(1);
}


/////////////////////////////////////////////////////////////
// Motor stuff
// Need to put in the correct values for the analogWrite commands
// in the goForward() and goBackward() functions so that the
// robot does what the functions are intended to do
int in1Pin = 10;      // right motor
int in2Pin = 11;      // right motor
int in3Pin = 6;       // left motor
int in4Pin = 5;       // left motor

void initMotor() {
  // controls for right motor 1
  pinMode(in1Pin, OUTPUT);
  pinMode(in2Pin, OUTPUT);
  // controls for left motor 2
  pinMode(in3Pin, OUTPUT);
  pinMode(in4Pin, OUTPUT);  

  //By default turn off both motors
  analogWrite(in1Pin, 0);
  analogWrite(in2Pin, 0);
  analogWrite(in3Pin, 0);
  analogWrite(in4Pin, 0);
}

void stopMotor(int delayTime = 0) {
  analogWrite(in1Pin, 0);
  analogWrite(in2Pin, 0);
  analogWrite(in3Pin, 0);
  analogWrite(in4Pin, 0);
  if (delayTime > 0) {
    delay(delayTime);
  }
}

void goForward(int delayTime = 0, int motorSpeed = 255) {
  // change the 0 and motorSpeed for each pair so that the robot will go forward
  motorSpeed = abs(motorSpeed); // maker sure value is positive
  analogWrite(in1Pin, 0);           // right motor
  analogWrite(in2Pin, motorSpeed);  // right motor
  analogWrite(in3Pin, motorSpeed);  // left motor
  analogWrite(in4Pin, 0);           // left motor
  if (delayTime > 0) {
    delay(delayTime);
    stopMotor();
  }
}

void goBackward(int delayTime = 0, int motorSpeed = 255) {
  // change the 0 and motorSpeed for each pair so that the robot will go backward
  motorSpeed = abs(motorSpeed); // maker sure value is positive
  analogWrite(in1Pin, motorSpeed);  // right motor
  analogWrite(in2Pin, 0);           // right motor
  analogWrite(in3Pin, 0);           // left motor
  analogWrite(in4Pin, motorSpeed);  // left motor
  if (delayTime > 0) {
    delay(delayTime);
    stopMotor();
  }
}

// you can use this to test your goForward() and goBackward() functions
void testMotors() {
  Serial.println("Go forward");
  goForward(1000);
  Serial.println("Go backward");
  goBackward(1000);
}

void setup() {
  Serial.begin(115200);
  digitalWrite(statusPin, HIGH);
  initMotor();
//  testMotors();
  initMPU();
  initPID();
//  testGyro();
}

void loop() {
  static float xyz[3];

  flashActivityLed(500, 150, 1);
  
  checkMPU(xyz);

  // get X-axis value and adjust the upright position to be at 180
  // choose the correct code from below - whether gyro is facing UP or DOWN

  // facing UP
  // this is for when the gyro is placed with the components side pointing UP
  pidInput = xyz[0] + 180;

  // facing DOWN
  // this is for when the gyro is placed with the components side pointing DOWN
  // pidInput = xyz[0];
  // if (pidInput < 0) pidInput = (180 + pidInput) + 180;

  
  pid.Compute();  // compute the new PID correction value
  Serial.print(pidInput); Serial.print(" => "); Serial.print(pidOutput);

  // control the motors depending on the robot tilt (X-axis)
  // depending on how you mounted the motors, you might have to
  // swap the forward and backward tests
  if (pidOutput < 0) {          // Falling towards the front
    Serial.println(" forward"); 
    goForward(0, -pidOutput);
  } else if (pidOutput > 0) {   // Falling towards the back
    Serial.println(" backward");
    goBackward(0, pidOutput);
  } else {
    Serial.println(" upright");
    stopMotor();
  }
  
}

Please start by reading this and consider editing your post to make it readable.

I am really sorry, I used the wrong command! I just fixed it

1 Like

The robot is not accelerating fast enough. It needs the bottom to accelerate faster than the top can fall. If your control software is not driving the motors as hard as possible, drive them harder. If you ARE driving them as hard as possible, your motors are under-powered. Perhaps higher drive voltage would help.

Assuming that the motor power is adequate, either the PID algorithm constants are not properly selected, or the angle measurement is not working correctly.

There is too little information in your post to say more.

Project - YouTube here there is a video I just recorded to showcase what is wrong with it

To obtain information that is actually useful for debugging your problem (the video is not useful), put in print statements to report the measured and true tilt angle and PID response for a variety of test positions.

I just attached a video of the robot to better showcase what is happening, I think it does not have to do with the motor because it always falls to the same side but when it hits something since it starts going full speed to that direction and does not stop

How do I do that

Serial.println(value);

Where "value" is some quantity of interest, and will help you understand what is going wrong.

Thank you! In what part of the code should I put it?

Put that statement in parts of the code where you want to know what quantity is in "value".