Gyro data lags using complementary filter for Balancing Robot.

Will post description of problem as reply.

#include "I2Cdev.h"
#include <PID_v1.h> // load PID Library
#include <math.h>

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
    #include "Wire.h"

#define   GYR_Y                 0                              // Gyro Y 
#define   ACC_Z                 1                              // Acc  Z 
#define   ACC_X                 2                              // Acc  X 

const int MPU_addr = 0x68;  // I2C address of the MPU-6050

// Sensor Variables
int sensorValue[3]  = {0, 0, 0};
int sensorZero[3]   = {0, 0, 0}; 
int pool = 4; // number of angle reads to average
int16_t ax, az, ay, ACCZ_force, ACCX_force, ACC_Angle, AccAngle;
int16_t gy, gz, gx, GYRO_rate;
double preAngle;

// Timing Variables
long STD_LOOP_TIME = 9;             
double lastLoopTime = STD_LOOP_TIME;
long lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

//Motor Variables
int EnA = 5, In1 = 6, In2 = 7; // set pins used for motor control (Motor 1)
int EnB = 9, In3 = 10, In4 = 11; // set pins used for motor control (Motor 2)

// PID Variabiles
double Kp, Ki, Kd, Setpoint = 0; // PID Variables
double PID_Speed, Angle = 0; // define PWM values sent to motor
double Motor_Min = 30, Motor_Max = 255;
int Sampling = 10; // set PID sampling in milliseconds

// Define PID 
PID Balance (&Angle, &PID_Speed, &Setpoint, Kp, Ki, Kd, REVERSE);

long Start, End;
void setup(){ Serial.begin(115200);
// join I2C bus (I2Cdev library doesn't do this automatically)
        Fastwire::setup(400, true);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
    Serial.println("Waking MPU6050 from Sleep Mode...");
// Set Set motor outputs 
pinMode(In1, OUTPUT);
pinMode(In2, OUTPUT);
pinMode(EnA, OUTPUT);

pinMode(In3, OUTPUT);
pinMode(In4, OUTPUT);
pinMode(EnA, OUTPUT);

// Set PID
Balance.SetMode(AUTOMATIC); // TURN ON PIDs
Balance.SetSampleTime(Sampling); // set PID sampling rate
Balance.SetOutputLimits(Motor_Min, Motor_Max); // set minimum to just below movement threshold

// Zero Out Sensors
void loop()  { // ****************Start Looo********************

// ********************* Actual Balance Bot Stuff *******************
updateSensors(); // gets sensor values
getAngle(); // computes -90 / +90 angle from sensor values 
selectDirection(); // - numbers, go forward; + numbers, go backwards
Angle = getPitch(); // Converts 16 bit, -90/90 variable to 8 bit, 0 - 90 double variable for PID
readPots(); // get Kp, Ki, and Kd from potentiometers
Balance.Compute(); // compute wheel speed using Angle as input and PID_Speed as output
powerWheels(); // Send PID_Speed to wheels

// ********************* print Debug info *************************************

// *********************** loop timing control **************************
 lastLoopUsefulTime = millis()-loopStartTime;
 if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
 lastLoopTime = millis() - loopStartTime;
 loopStartTime = millis();

// ************************Land o' Functions******************************

// *********Debugging Print Functions***********
void serialOut_timing() {
static int skip=0;
 //if(skip++==5) {                         // display every 500 ms (at 100 Hz)                              
 //  skip = 0;
   Serial.print(lastLoopUsefulTime);    Serial.print(",");
   Serial.print(lastLoopTime);          Serial.print("\n");

void printGYAC() {
//Serial.print("Gyro Y: "); Serial.print(GYRO_rate); Serial.println(" - ");
//Serial.print("Acc Y: "); Serial.print(ACCX_force); Serial.print(" - ");
//Serial.print("Acc Z: "); Serial.print(ACCZ_force); Serial.print(" - ");
//Serial.print("Angle: ");
Serial.print(Angle) ;
Serial.print("  -  ");

void serialOut_raw() {
static int skip=0;
 if(skip++==5) {                                                        
   skip = 0;
   Serial.print("ACC_X:"); Serial.print(sensorValue[ACC_X]); 
   Serial.print(" ACC_Z:"); Serial.print(sensorValue[ACC_Z]);     
   Serial.print(" GYR_Y:");  Serial.println(sensorValue[GYR_Y]); 
 void printMotorvariables(){
Serial.print(" - ");
Serial.print(" - ");
Serial.print(" - ");
Serial.print(" - ");


//*************Sensor Functions*************
void calibrateSensors() {  // Set sensor baseline to zero
 long v = 0;
 long v1 = 0;
 long v2 = 0;
   for(int i=0; i<200; i++) {
   v += gy;
   v1 += az;
   v2 += ax;
   sensorZero[GYR_Y] = v/200;
   sensorZero[ACC_Z] = v1/200;
   sensorZero[ACC_X] = v2/200;                                            
sensorZero[ACC_Z] -= 16384; // set z-accel to 1g                       

void updateSensors() { // data acquisition
 long v = 0;
 long v1 = 0;
 long v2 = 0;
   for(int i=0; i<3; i++) { 
v += gy;
 v1 += az;
 v2 += ax;
// Subtract calibrateSensor() data from readSensor() data
   sensorValue[GYR_Y] = (v/3) - sensorZero[GYR_Y];
   sensorValue[ACC_Z] = (v1/3)- sensorZero[ACC_Z];
   sensorValue[ACC_X] = (v2/3)- sensorZero[ACC_X];
void readSensor(){

 // Read Accellerometer X (forward and back)
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.requestFrom(MPU_addr, 2,true);  // request a total of 2 registers
  ax =<<8|;  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)        
 // Read Accelerometer Z (gravity)
  Wire.write(0x3F);  // starting with register 0x3F (ACCEL_ZOUT_H)
  Wire.requestFrom(MPU_addr, 2,true);  // request a total of 2 registers
  az =<<8|;  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

 // Read Gyroscope Y (pitch)
  Wire.write(0x45);  // starting with register 0x45 (GYRO_YOUT_H)
  Wire.requestFrom(MPU_addr, 2,true);  // request a total of 2 registers
  gy =<<8|;  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)

// *********************Math Functions****************
float getGyroRate() { 
 return int(sensorValue[GYR_Y] / 131);  // @ gyro sensitivity of +/-250deg = 131LSB per degree

float getAccAngle() {  // Get arctangent of sensor values X and Z (postion in radians) then convert to Euler angle           
 return ((atan2(sensorValue[ACC_X],sensorValue[ACC_Z])+PI)*RAD_TO_DEG)-180; // 

float angle(){
  float dt = lastLoopTime/1000; // convert lastLoopTime to ms
return 0.98*(preAngle + (GYRO_rate*dt)) + (0.02*ACC_Angle);// Complementary Filter

void getAngle(){ // Does above in right order
  GYRO_rate = getGyroRate();   
  ACC_Angle = getAccAngle();  
  Angle = angle();
  preAngle = Angle; // sets preAngle to current Angle setting, which will be previous
  // angle next time around

double getPitch(){
  return abs(Angle); // Converts 16 bit, -180/180 int variable to 8 bit, 0 - 180 double variable for PID

//***********Motor Functions***************
void selectDirection(){ // select which direction the motor should go
if (Angle <= 0){

if (Angle > 0) {
void goForward(){
  // if leaning Forward, set motors to Forward
digitalWrite (In1, LOW);
digitalWrite (In2, HIGH);
digitalWrite (In3, LOW);
digitalWrite (In4, HIGH);


void goBackwards()  { // If leaning Backwards, set motors to Backwards

digitalWrite (In1, HIGH); 
digitalWrite (In2, LOW);
digitalWrite (In3, HIGH);
digitalWrite (In4, LOW);


//**********PID Functions**************
void readPots(){  // Read values from potentiameteres at A0-2 to get Kp, Ki, and Kd
  Kp  = analogRead(A0)*.01; //range from 0 - 10
  Ki = analogRead(A1)*.01; // range from 0 - 10
  Kd = analogRead(A2)*.1; //  range from 0 - 100
Balance.SetTunings(Kp, Ki, Kd); // set Kp/i/d to values from potentiometers

// Send PID_Speed as PWM to motor controler
void powerWheels(){
analogWrite(EnA, (PID_Speed + 22)); // +22 repesents difference between motors (investigate this)
analogWrite(EnB, PID_Speed);

Code took up almost all of the character limit, so I couldn't describe my problem in that post.

When I set the tunings to 0.98 g / 0.02 A the angle information from the filter lags behind the actual robot by about 0.5-1 seconds. The Accelerometer data seems fine. The raw gyro data and the data converted to deg/sec also seems OK. It's only when plugging data into the filter that it becomes slow. Also, this happens regardless of whether I'm monitoring data using the serial port as evidenced by the robot lagging in reversing direction when tilted past vertical. The angle eventually gets to the correct value, it's just slow.

I've tried not averaging 3 readings to speed things up and reducing the loop time to 5ms, but that didn't work. As the code currently reads, a loop takes about 6-8ms and is timed to repeat every 9ms.

Printing is very slow. Eliminate it entirely and see what happens.

jremington: Printing is very slow. Eliminate it entirely and see what happens.

Tried that. Doesn't work. Even with the print functions commented out and the USB cable disconnected, there's a delay in the wheels reversing direction when the robot goes past vertical.

When I set the tunings to 0.98 g / 0.02 A

What about other settings? 2% is a tiny, almost negligible influence.

float dt = lastLoopTime/1000; // convert lastLoopTime to ms

........ lastLoopTime is already in milliseconds to begin with, right? Probably should say 'convert to seconds'....which probably won't change anything since the complementary filter equation appears to require units of seconds already.

Yep..... definitely avoid too many serial prints in the control program. Maybe avoid altogether.

Well, commenting out all the print functions didn't have any effect. Whatever I'm doing each loop, it's taking less than the standard loop time of 9ms, so any more speed won't get me to do another loop quicker. A 5 ms loop didn't help either. The raw data responds quickly (or at least much quicker than the end product), so it's just when I convert it to actual degrees moved in 0.009 seconds that it gets backed up. But, backed up where? There's no buffer. If Gyro Rate has returned to 0deg/sec, why would the angle keep changing? The angle should stop changing when the gyro says it's not moving. Right? It's like I'm making a math error somewhere.

I tried changing the filter to a 50/50 mix. It was faster, but very noisy. 70 gyro /30 accel worked a little better, but was slow and noisy. I was under the impression that the gyro was supposed to be fast and accurate, but the slow and noisy acceleration data was needed to compensate for gyro drift.

The gyro information should be the fast component, not the slow. The accelerometer data should be there to correct for long term drift only.

I think you've got the two mixed up!