Self-Balancing Robot Code

Other Hardware Robotics
1

I am using an Arduino Uno, MPU6050 and dc motors to create a 2-wheel self-balancing robot. I am stuck in part of my code where I calculate for the angle of the IMU, (thus the angle that the robot is leaning in). I feel it has something to do with the timing loop, but honestly do not know what is wrong. I used a euler formula to calcuate the angle of the accelerometer. "gForce" values are raw accelerometer values divided by 16384.0, and "gyroX" is the raw gyro value, divided by 131.0, as per the mpu6050 datasheet. The attached image is the graph I get when plotting "Error". When sitting still and upright, its reading a very shaky value around -2.7 degrees, and when moved to be on an angle and stay at an angle, it does stay there. This leads me to believe that the euler formula part is correct, however perhaps I need to use offsets for my raw values so that the shakiness is dissipated?

Project Code:

/////PID IMPLEMENTATION/////
//Error
float Error;
float ErrorT;
float LastError;
//PID
float Proportional;
float Integral;
float Derivative;
double P_I_D;
float Setpoint = 0;
//angle
float rad_to_deg = 180/3.141592654;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];
//Timing
float time;
int SampleTime = 1000;
unsigned long lastTime;
double lastInput;
//motors
int motorVal;
float elapsedTime, timePrev;
int i;

void calculatePID() {
//Timing loop
  timePrev = time;  // the previous time is stored before the actual time read
  time = millis();  // actual time read
  elapsedTime = (time - timePrev); 

//Filtering to get rid of gyro drift 
  Acceleration_angle[0] = atan((gForceY)/(sqrt(pow((gForceX),2) + pow((gForceZ),2))))*rad_to_deg;
  Gyro_angle[0] = gyroX / 131.0; 
  Total_angle[0] = 0.02*(Total_angle[0]*10 + Gyro_angle[0]) + 0.98*(Acceleration_angle[0]);

  Error =  Total_angle[0] - Setpoint;

  double dInput = (Total_angle[0] - lastInput);

//Calculate PID
  Proportional = Error * Kp;
  Integral = ErrorT * Ki;
  Derivative = dInput * Kd;
 
  P_I_D = Proportional + Integral + Derivative;

//remember for next time
  timePrev = time;
  lastInput = Total_angle[0];
  
}

Theory: PID control arduino drones mpu6050 mpu9250 gyro accelerometer euler msp432 rtos

Thanks again.

Screen Shot 2018-04-28 at 5.20.20 PM.png
Screen Shot 2018-04-28 at 5.20.20 PM.png515×533 17.9 KB

2

Please read and follow the directions in the "How to use this forum" post.

Post the code, using code tags.

I used a euler formula to calcuate the angle of the accelerometer.

What formula, exactly? Post a link to the theory.

You may find this page useful. How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot

3

The following is not an angle. It is a rate of rotation.

Gyro_angle[0] = gyroX / 131.0;

Always post ALL the code.