PID controller with accelerometer and servo motor

Hey, I am a beginner and I am trying to make a code with PID controller. I have an MPU6050 accelerometer and a servo motor. I want the servo motor to rotate accordingly when the accelerometer leans in one or other direction. And I want this to be done through a PID controller which will calculate the error and rotate the servo until the angle of the accelerometer is back at 0 degrees. I found a code for the accelerometer online and it works fine (gives me the angle in degrees). I have also found a useful website which explains how to make a PID controller. However, I did not manage to find out how I am going to tell the servo motor to rotate until the error of the accelerometer gets to 0.

I have attached the .ino file with my code!

I am begging for some help here!

Thanks :slight_smile:

PID_accelerometer.ino (9.88 KB)

Please read the advice in the beginning, how to post code etc. Use code tags, </>. Some members use smartphones to read topics and have no IDE to handle code.

Here is the code:

#include <PID_v1.h>
#include <Wire.h>
#include <Servo.h>

Servo steer_servo;

//Gyro Variables
float elapsedTime, time, timePrev;       
int gyro_error=0;                         
float Gyr_rawX, Gyr_rawY, Gyr_rawZ;     
float Gyro_angle_x, Gyro_angle_y;         
float Gyro_raw_error_x, Gyro_raw_error_y; 

//Acc Variables
int acc_error=0;                         
float rad_to_deg = 180/3.141592654;      
float Acc_rawX, Acc_rawY, Acc_rawZ;   
float Acc_angle_x, Acc_angle_y;         
float Acc_angle_error_x, Acc_angle_error_y; 

float Total_angle_x, Total_angle_y;

/*working variables for PID*/
unsigned long lastTime;
double Input, Output, Setpoint;
double ITerm, lastInput;
double kp, ki, kd;
int SampleTime = 1000; //1 sec
double outMin, outMax;
bool inAuto = false;

void setup()
{
  // put your setup code here, to run once:
  Wire.begin();                           //begin the wire comunication
  
  Wire.beginTransmission(0x68);           //begin, Send the slave adress (in this case 68)              
  Wire.write(0x6B);                       //make the reset (place a 0 into the 6B register)
  Wire.write(0x00);
  Wire.endTransmission(true);             //end the transmission
  //Gyro config
  Wire.beginTransmission(0x68);           //begin, Send the slave adress (in this case 68) 
  Wire.write(0x1B);                       //We want to write to the GYRO_CONFIG register (1B hex)
  Wire.write(0x10);                       //Set the register bits as 00010000 (1000dps full scale)
  Wire.endTransmission(true);             //End the transmission with the gyro
  //Acc config
  Wire.beginTransmission(0x68);           //Start communication with the address found during search.
  Wire.write(0x1C);                       //We want to write to the ACCEL_CONFIG register
  Wire.write(0x10);                       //Set the register bits as 00010000 (+/- 8g full scale range)
  Wire.endTransmission(true); 

  Serial.begin(9600);                     //Remember to set this same baud rate to the serial monitor  
  time = millis();                        //Start counting time in milliseconds

/*Here we calculate the acc data error before we start the loop
 * I make the mean of 200 values, that should be enough*/
  if(acc_error==0)
  {
    for(int a=0; a<200; a++)
    {
      Wire.beginTransmission(0x68);
      Wire.write(0x3B);                       //Ask for the 0x3B register- correspond to AcX
      Wire.endTransmission(false);
      Wire.requestFrom(0x68,6,true); 
      
      Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0 ; //each value needs two registres
      Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;
      Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0 ;

      /*---X---*/
      Acc_angle_error_x = Acc_angle_error_x + ((atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg));
      /*---Y---*/
      Acc_angle_error_y = Acc_angle_error_y + ((atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg)); 
      
      if(a==199)
      {
        Acc_angle_error_x = Acc_angle_error_x/200;
        Acc_angle_error_y = Acc_angle_error_y/200;
        acc_error=1;
      }
    }
  }//end of acc error calculation   


/*Here we calculate the gyro data error before we start the loop
 * I make the mean of 200 values, that should be enough*/
  if(gyro_error==0)
  {
    for(int i=0; i<200; i++)
    {
      Wire.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68) 
      Wire.write(0x43);                        //First adress of the Gyro data
      Wire.endTransmission(false);
      Wire.requestFrom(0x68,4,true);           //We ask for just 4 registers 
         
      Gyr_rawX=Wire.read()<<8|Wire.read();     //Once again we shif and sum
      Gyr_rawY=Wire.read()<<8|Wire.read();
   
      /*---X---*/
      Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX/32.8); 
      /*---Y---*/
      Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY/32.8);
      if(i==199)
      {
        Gyro_raw_error_x = Gyro_raw_error_x/200;
        Gyro_raw_error_y = Gyro_raw_error_y/200;
        gyro_error=1;
      }
    }
  }//end of gyro error calculation
  steer_servo.attach(9);
}

void loop()
{
  // put your main code here, to run repeatedly:
  accelerometer();  
}

void accelerometer()
{
  timePrev = time;                        // the previous time is stored before the actual time read
  time = millis();                        // actual time read
  elapsedTime = (time - timePrev) / 1000; //divide by 1000 in order to obtain seconds
  
  //////////////////////////////////////Gyro read/////////////////////////////////////

  Wire.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68) 
  Wire.write(0x43);                        //First adress of the Gyro data
  Wire.endTransmission(false);
  Wire.requestFrom(0x68,4,true);           //We ask for just 4 registers
        
  Gyr_rawX=Wire.read()<<8|Wire.read();     //Once again we shif and sum
  Gyr_rawY=Wire.read()<<8|Wire.read();
  /*Now in order to obtain the gyro data in degrees/seconds we have to divide first
  the raw value by 32.8 because that's the value that the datasheet gives us for a 1000dps range*/
  /*---X---*/
  Gyr_rawX = (Gyr_rawX/32.8) - Gyro_raw_error_x; 
  /*---Y---*/
  Gyr_rawY = (Gyr_rawY/32.8) - Gyro_raw_error_y;
  

  Gyro_angle_x = Gyr_rawX*elapsedTime;

  Gyro_angle_y = Gyr_rawY*elapsedTime;    
  
 

  Wire.beginTransmission(0x68);     //begin, Send the slave adress (in this case 68) 
  Wire.write(0x3B);                 //Ask for the 0x3B register- correspond to AcX
  Wire.endTransmission(false);      //keep the transmission and next
  Wire.requestFrom(0x68,6,true);    //We ask for next 6 registers starting withj the 3B
  Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0 ; //each value needs two registres
  Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;
  Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0 ; 
  /*Now in order to obtain the Acc angles we use euler formula with acceleration values
  after that we substract the error value found before*/  
  /*---X---*/
  Acc_angle_x = (atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_x;
  /*---Y---*/
  Acc_angle_y = (atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_y;    


  Total_angle_x = 0.98 *(Total_angle_x + Gyro_angle_x) + 0.02*Acc_angle_x;

  Total_angle_y = 0.98 *(Total_angle_y + Gyro_angle_y) + 0.02*Acc_angle_y;
  

  Serial.print("Xº: ");
  Serial.print(Total_angle_x);
  Serial.print("   |   ");
  Serial.print("Yº: ");
  Serial.print(Total_angle_y);
  Serial.println(" ");
}

void Compute()
{
  if(!inAuto) return;
  unsigned long now = millis();
  int timeChange = (now - lastTime);
  if(timeChange>=SampleTime)
  {
    /*Compute all the working error variables*/
    double error = Setpoint - Input;
    ITerm += (ki * error);
    if(ITerm > outMax) ITerm = outMax;
    else if(ITerm < outMin) ITerm = outMin;
    double dInput = (Input - lastInput);
    
    /*Compute PID Output*/
    Output = kp * error + ITerm - kd * dInput;
    if(Output > outMax) Output = outMax;
    else if(Output < outMin) Output = outMin;
 
    /*Remember some variables for next time*/
    lastInput = Input;
    lastTime = now;
  }
}

void SetTunings(double Kp = 10, double Ki = 0, double Kd = 0)
{
  double SampleTimeInSec = ((double)SampleTime)/1000;
  kp = Kp;
  ki = Ki * SampleTimeInSec;
  kd = Kd / SampleTimeInSec;
}
 
void SetSampleTime(int NewSampleTime)
{
  if (NewSampleTime > 0)
  {
    double ratio  = (double)NewSampleTime/(double)SampleTime;
    ki *= ratio;
    kd /= ratio;
    SampleTime = (unsigned long)NewSampleTime;
  }
}

void SetOutputLimits(double Min, double Max)
{
  if(Min > Max) return;
  outMin = Min;
  outMax = Max;
    
  if(Output > outMax) Output = outMax;
  else if(Output < outMin) Output = outMin;
 
  if(ITerm > outMax) ITerm = outMax;
  else if(ITerm < outMin) ITerm = outMin;
}

void SetMode(int Mode)
{
  bool newAuto = (Mode == AUTOMATIC);
  if(newAuto && !inAuto)
  {  /*we just went from manual to auto*/
    Initialize();
  }
  inAuto = newAuto;
}
 
void Initialize()
{
  lastInput = Input;
  ITerm = Output;
  if(ITerm > outMax) ITerm = outMax;
  else if(ITerm < outMin) ITerm = outMin;
}

I'm not at all experienced in PID and servo running. However, looking into the code I see that steer_servo is only called once, in the end of "Setup". Don't You need to call that routine somewhere inside loop()? I miss the updating of the servo.
Setup is executed only at start up and loop() is the main program going around and around.

Using the PID library to control a servo is usually overkill. A servo is already a closed loop control system and wrapping it with a PID controller is only needed when either you're trying to control a nonlinear system or you're trying to get extreme precision.

In either of those two cases, though, it's better to use a DC motor with a PID controller instead of a servo.

What you can do if you do not want to use a DC motor is the following:

Build the system with the servo and attach the IMU to the servo head (or whatever the servo head is controlling). Measure the offset angle and command the servo to compensate for that exact angle offset. It's basically a P controller that lets the servo do what it does best - heavy lifting closed loop control. Easy peasy

Curious, what is this project about?

Build the system with the servo and attach the IMU to the servo head (or whatever the servo head is controlling). Measure the offset angle and command the servo to compensate for that exact angle offset. It's basically a P controller that lets the servo do what it does best - heavy lifting closed loop control. Easy peasy

Curious, what is this project about?

It is an autonomous bicycle project so basically I want the servo motor to turn the steer of the bicycle when bicycle leans in any direction.
So what you mean is that I don't need the PID controller and I just need to instruct the servo to rotate when the accelerometer angle is not 0?
Is this all PID code unnecessary?

And if I am going to do that how can I tell the servo to rotate more when the bicycle lean angle is increasing but when start to decreasing instruct to start rotating back to its starting position?

Thanks!

chrismil:
It is an autonomous bicycle project so basically I want the servo motor to turn the steer of the bicycle when bicycle leans in any direction.

Oooh, I understand now. Because there isn't a "one to one" relationship between servo angle and roll angle (what you are measuring with the IMU), this is actually a non-linear system and you will need a PID controller.

I am wondering though, have you done any testing to see if your servo is strong enough to steer a bicycle at all - even without a PID loop? Can it turn the front wheel under full load standing still (bike stationary)?

In order to implement a PID loop you have to either implement the functions of an existing PID library or write your own. Neither are terribly difficult, but you shouldn't do both in the same sketch. For example I see in your previously posted code that you included a PID library yet also wrote your own PID functions - pick one.

Once you decide whether you'll use a library or write your own PID algorithms, you need to decide a few things. Some of these are obvious, but it's a good exercise nonetheless:

  • What is my setpoint? A setpoint is the desired state of the system. In your case, your setpoint will be the bank roll angle to be 0 degrees (level).
  • What is my feedback? Feedback comes in the form of sensors and. This feedback should be in the same units as your setpoint
  • What is my error? Error is always, always, always, setpoint minus feedback. This subtraction calculation is why the units of the setpoint and feedback must match to make sense.
  • What are my gains? Kp, Ki, and Kd all have their own nuances and uses to make the system respond the way you want it to. Usually tuning is just a guessing game that involves a lot of trial and error.

The most common way to tune the gains is to zero Ki and Kd, start with Kp small and slowly increase Kp until your system gently oscillates. Then increase Kd until the oscillations dampen out. Lastly increase Ki slightly until the offset goes away.

I've never dealt with designing a control system for bike steering so tuning your PID might be different than what I described above, but it gives you a starting point.

PID Video