Self balancing robot

Hi guys, I am now in my last year of the middle school in the section industrial sciences.
I need to make a self balancing robot with 2 other persons.
We use the arduino uno + motorshield and the MPU6050.
We already have a big program but there are some faults in it, that we can’t figure out.
The most problems are with the PID-controller.

The first problem is the setpoint. We have now set it on 600 because when we measured the values, while the robot was flat, were around 600.

//initialize the variables we're linked to
      Input = analogRead(0);
      Setpoint = 600;

Another problem is the input, we need to read the values. After that we want to calculate the gap. The gap is setpoint - measured values. If the gap is smaller than 50 but greater than zero, than we need to drive the robot forwards. If the gap is greater then 50 then we need to react fast. If the gap is greater than -50 then we need to drive backwards and if it is smaller than -50 than we also need to react fast.
To define the input we used this code.

Input = ((i2cData[2] << 8) | i2cData[3]);

If you have time left we will appriciate that you also check our complementary filter or the whole program.

Thank you in advanced.

This is the whole program

[code]#include <Wire.h>
#include <PID_v1.h>
#include "Kalman.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include <PID_v1.h>

MPU6050 accelgyro;

int motorAD = 12; //motor a dir
int motorBD = 13; //motor b dir
int motorAB = 9; //motor a brake
int motorBB = 8; // motor b brake




int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
int16_t ax, ay, az;
int16_t gx, gy, gz;


double accXangle, accYangle; // Angle calculate using the accelerometer
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double velocity;





const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication


uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) 
{
  return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) 
{
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data, length);
  uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
  if (rcode) 
    {
      Serial.print(F("i2cWrite failed: "));
      Serial.println(rcode);
    }
  return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}

uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) 
{
  uint32_t timeOutTimer;
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
  if (rcode)
    {
      Serial.print(F("i2cRead failed: "));
      Serial.println(rcode);
      return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
    }

  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
  for (uint8_t i = 0; i < nbytes; i++) 
    {
      if (Wire.available())
      data[i] = Wire.read();
      else 
        {
          timeOutTimer = micros();
          while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
          if (Wire.available())
          data[i] = Wire.read();
          else
             {
                Serial.println(F("i2cRead timeout"));
                return 5; // This error value is not already taken by endTransmission
             }
        }
    }
  return 0; // Success
}





uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
float output = 0.0; 



//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.5, aggKd=2;
double consKp=1, consKi=0.05, consKd=0.25;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);



void setup() 
  { 
   
    Serial.begin(9600); 
    pinMode(motorAD, OUTPUT);
    pinMode(motorAB, OUTPUT);
    pinMode(motorBD, OUTPUT);
    pinMode(motorBB, OUTPUT);
    
    
    
    
    Wire.begin();
    i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
    i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
    i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
    i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
    while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
    while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode 
  
    while(i2cRead(0x75,i2cData,1));
    if(i2cData[0] != 0x68) // Read "WHO_AM_I" register
      { 
        Serial.print(F("Error reading sensor"));
        while(1);
      }
  
    delay(100); // Wait for sensor to stabilize

  
  
  
    while(i2cRead(0x3B,i2cData,6));
    accX = ((i2cData[0] << 8) | i2cData[1]);
    accY = ((i2cData[2] << 8) | i2cData[3]);
    accZ = ((i2cData[4] << 8) | i2cData[5]);
    // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
    // We then convert it to 0 to 2? and then from radians to degrees
    accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
    accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  
  
    gyroXangle = accXangle;
    gyroYangle = accYangle;
    compAngleX = accXangle;
    compAngleY = accYangle;
  
    timer = micros();
    
    
    {
      //initialize the variables we're linked to
      Input = analogRead(0);
      Setpoint = 600;

      //turn the PID on
      myPID.SetMode(AUTOMATIC);
    }
  }
  
  
  
  
  void loop() 
  {
    
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    ax = map(ax, -17000, 1700, -1500, 1500);
      
      /* Update all the values */  
     // while(i2cRead(0x3B,i2cData,14));
     //LEES WAARDEN
    accX = ((i2cData[0] << 8) | i2cData[1]);
    accY = ((i2cData[2] << 8) | i2cData[3])/1000;
    accZ = ((i2cData[4] << 8) | i2cData[5]);
    gyroX = ((i2cData[8] << 8) | i2cData[9]);
    gyroY = ((i2cData[10] << 8) | i2cData[11]);
    gyroZ = ((i2cData[12] << 8) | i2cData[13]);
  
    //COVERTEER RAD -> GRADEN             covert RAD -> Degrees
    accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
    accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  
    //GRADEN->GRADEN/SEC      degrees-> degrees/sec
    double gyroXrate = (double)gyroX/131.0; 
    double gyroYrate = -((double)gyroY/131.0);
    gyroXangle += gyroXrate*((double)(micros()-timer)/1000000);
  
  

    //BEREKEN DE HOEK / calculate angle
    compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); 
    timer = micros();
  
  
 /* 
   if (ax > 1000)
        {
          digitalWrite(motorAD, HIGH); 
          digitalWrite(motorBD, LOW);
          analogWrite(3, 125);
          //analogWrite(11, 125);
        }
      
      if (ax < -1000)
        {
       
          digitalWrite(motorBD, HIGH);
          digitalWrite(motorAD, LOW);
          //analogWrite(3, 125);
          analogWrite(11, 125);
       
        }
  */        
    {
    Input = ((i2cData[2] << 8) | i2cData[3]);
  
      double gap = abs(Setpoint-Input); //distance away from setpoint
      if(gap<=50 && gap > 0)
        {  //we're close to setpoint, use conservative tuning parameters
          myPID.SetTunings(consKp, consKi, consKd);
        }
      else
        {
           //we're far from setpoint, use aggressive tuning parameters
           myPID.SetTunings(aggKp, aggKi, aggKd);
         }
  
      myPID.Compute();
      {
          digitalWrite(motorAD, HIGH); 
          digitalWrite(motorBD, LOW);
          analogWrite(11, Output);
          //analogWrite(11, 125);
          
          digitalWrite(motorAD, LOW); 
          digitalWrite(motorBD, HIGH);
          analogWrite(3, Output);
          //analogWrite(11, 125);
        }
   
   
   
   
   
      if(gap >= -50 && gap < 0)
        {  //we're close to setpoint, use conservative tuning parameters
          myPID.SetTunings(consKp, consKi, consKd);
        }
      else
        {
           //we're far from setpoint, use aggressive tuning parameters
           myPID.SetTunings(aggKp, aggKi, aggKd);
         }
  
      myPID.Compute();
      {
          digitalWrite(motorAD, LOW); 
          digitalWrite(motorBD, HIGH);
          analogWrite(3, Output);
          //analogWrite(11, 125);
          
          digitalWrite(motorAD, HIGH); 
          digitalWrite(motorBD, LOW);
          analogWrite(11, Output);
          //analogWrite(11, 125);
        }
   
      }
  }

[/code]

I would start by setting the PID Output range to -255 - 255. That will give you both speed and direction.

You should not need different PID tunings for different ranges of data. One PID tuning should work. The PID will naturally increase the power to the motors if the error value (Input - Setpoint) is higher.

Hi Johnwasserr,

Thanks a lot for your prompt reply!

Could you tell me if I need to use the output range before the void setup, in the void setup or in the void loop?

Is below command correct? SetOutputLimits(-255, 255)

Unfortunatly I will not be able to test my program till Monday... .

That looks correct to me. When Output is >= 0 use it for forward speed. When Output is <0 use -Output for reverse speed.

OK, thank you.

I will try it on monday, but when we are going to try it I also need to know what I need to write for the input.

We now have this:
Input = ((i2cData[2] << 8) | i2cData[3]); //read the y-values of the accelerometer.

Is this correct?

Thanks in advance.

CptNmars:
We now have this:

Input = ((i2cData[2] << 8) | i2cData[3]); //read the y-values of the accelerometer.

Is this correct?

That is probably correct if the Y axis is pointing forward to backward. If the robot tips toward the -Y side the gravity will produce a negative acceleration on the Y axis. If the robot tips to the +Y side the gravity will produce a positive acceleration on the Y axis. That should be sufficient to allow the PID to correct for tilt.

If you have a spare potentiometer around you may want to add a knob to adjust the setpoint. That will be faster than having to upload the sketch every time you want to tweak the setpoint. Connect the pot between +5 and Ground and connect the wiper to an analog input. That will give you a value between 0 and 1023 you can use to adjust the Setpoint in setup();

float Setpoint = 600;
void setup() {
.
.
.
Setpoint += (analogRead(A0) - 512) / 10;  // Adjustable between about 550 and 650
}

Just tweak the knob and hit Reset to use the new value.

We have now tested the program, but it won’t work.
We also changed the code a little bit but it still doesn’t work.

Underneath you can see our program for the PID.

#include <PID_v1.h>
//Define Variables we'll be connecting to
double  Setpoint, Input, Output;

//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.05, consKd=0.25;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);




void setup()

{
  //initialize the variables we're linked to
  Input = ((i2cData[2] << 8) | i2cData[3]); //read the y-values of the accelerometer.
  Setpoint = 600;  // Adjustable between about 550 and 650

  //turn the PID on
  myPID.SetMode(AUTOMATIC);

  myPID.SetOutputLimits(-255, 255);


}

void loop()
{
  Input = ((i2cData[2] << 8) | i2cData[3]); //read the y-values of the accelerometer.
  
  double gap = abs(Setpoint-Input); //distance away from setpoint
  if(gap<50)
  {  //we're close to setpoint, use conservative tuning parameters
    myPID.SetTunings(consKp, consKi, consKd);
  }
  else
  {
     //we're far from setpoint, use aggressive tuning parameters
     myPID.SetTunings(aggKp, aggKi, aggKd);
  }
  
  myPID.Compute();
  if (Output >= 0)
  {
    analogWrite(3,Output);
  }
  else
  {
    analogWrite(3,-Output);
  }
}
  myPID.Compute();
  if (Output >= 0)
  {
    analogWrite(3,Output);
  }
  else
  {
    analogWrite(3,-Output);
  }

Where do you set the motor controller for Forward and Reverse?

We use this to control the motors.

myPID.Compute();
      if (Output >= 0)
      {
          digitalWrite(motorAD, HIGH); //AD= motor A direction
          digitalWrite(motorBD, LOW); //BD = motor B direction
          analogWrite(11, Output);
         
          
        }
       else
        {
          digitalWrite(motorAD, LOW); 
          digitalWrite(motorBD, HIGH);
          analogWrite(3, Output);
          
          
          
        }

CptNmars: We use this to control the motors.

myPID.Compute();
      if (Output >= 0)
      {
          digitalWrite(motorAD, HIGH); //AD= motor A direction
          digitalWrite(motorBD, LOW); //BD = motor B direction
          analogWrite(11, Output);
        }
       else
        {
          digitalWrite(motorAD, LOW); 
          digitalWrite(motorBD, HIGH);
          analogWrite(3, Output);
        }

Why does one direction use Pin 11 for speed and the other direction use Pin 3 for speed?

Oh sorry I copied the wrong part.

myPID.Compute();
      if (Output >= 0)
      {
          digitalWrite(motorAD, HIGH); //AD= motor A direction
          digitalWrite(motorBD, LOW); //BD = motor B direction
          analogWrite(11, Output);
          analogWrite(3, Output);
        }
       else
        {
          digitalWrite(motorAD, LOW); 
          digitalWrite(motorBD, HIGH);
          analogWrite(3, Output);
          analogWrite(11, Output);
        }

Pin 3 is the speed for motor A en pin 11 is the speed for motor B

CptNmars: Oh sorry I copied the wrong part.

myPID.Compute();
      if (Output >= 0)
      {
          digitalWrite(motorAD, HIGH); //AD= motor A direction
          digitalWrite(motorBD, LOW); //BD = motor B direction
          analogWrite(11, Output);
          analogWrite(3, Output);
        }
       else
        {
          digitalWrite(motorAD, LOW); 
          digitalWrite(motorBD, HIGH);
          analogWrite(3, Output);
          analogWrite(11, Output);
        }

Pin 3 is the speed for motor A en pin 11 is the speed for motor B

This time you forgot to use -Output when Output is less than 0.

Why is that fault. We say that when output is >= 0 that motor A and B needs to turn forward with the speed of the Output and else that motor A en B needs to turn backwards with the speed of the Output.

CptNmars: Why is that fault. We say that when output is >= 0 that motor A and B needs to turn forward with the speed of the Output and else that motor A en B needs to turn backwards with the speed of the Output.

analogWrite() only works across the range 0 to 255. If you give it a negative number things won't work right. You have to separate Speed (0-255) and Direction (Forward or Backward).