Calculating z axis angular displacement with Mpu6050, Drifting

Hi, I hope this is the correct forum.
I am making an autonomous robot for a study project and thought about using a MPU6050 for getting the rotation around the z-axis.
I have this code, but the angleZ variable will very slowly increase when the robot is standing still without any disturbance. The increase is around 0.01 per reading.

Have some of you experienced the same problem, if yes, how did you solve it?
The idea is to include the readings from the encoders to get the rotation and then compare the two readings.

Thank you in advance

  • Chris
//=================================== --------------------------- ===================================
//=================================== -------- LIBRARIES -------- ===================================
//=================================== --------------------------- ===================================
 
#include <Wire.h>
 
//=================================== --------------------------- ===================================
//=================================== -------- VARIABLES -------- ===================================
//=================================== --------------------------- ===================================
 
int gyroZraw;
float gyroZerror, gyroZcorr, angleZ;
float previousTime, currentTime, deltaTime;
 
//=================================== --------------------------- ===================================
//=================================== ---------- SETUP ---------- ===================================
//=================================== --------------------------- ===================================
 
void setup() {
  Wire.begin();
  Serial.begin(9600);
  MPU6050_calibration();
 
}
 
//=================================== --------------------------- ===================================
//=================================== ---------- LOOP ----------- ===================================
//=================================== --------------------------- ===================================
 
void loop() {
 
  MPU6050_calculator();
}
 
//=================================== --------------------------- ===================================
//=================================== ------- MPU6050 DATA ------ ===================================
//=================================== --------------------------- ===================================
 
void MPU6050_data(){                                             //Subroutine for reading the raw gyro and accelerometer data
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x47);                                                    //Send the requested starting register
  Wire.endTransmission();                                              //End the transmission
  Wire.requestFrom(0x68,2);                                           //Request 14 bytes from the MPU-6050
  while(Wire.available() < 2);                                        //Wait until all the bytes are received
  gyroZraw = Wire.read()<<8|Wire.read();                                  //Add the low and high byte to the gyro_z variable
}
 
//=================================== --------------------------- ===================================
//=================================== ---- MPU6050 REGISTERS ---- ===================================
//=================================== --------------------------- ===================================
 
void MPU6050_registers(){
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x6B);                                                    //Send the requested starting register
  Wire.write(0x00);                                                    //Set the requested starting register
  Wire.endTransmission();                                              //End the transmission
  //Configure the gyro (500dps full scale)
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1B);                                                    //Send the requested starting register
  Wire.write(0x08);                                                    //Set the requested starting register
  Wire.endTransmission();                                              //End the transmission
}
 
//=================================== --------------------------- ===================================
//=================================== --- MPU6050 CALIBRATION --- ===================================
//=================================== --------------------------- ===================================
 
void MPU6050_calibration(){
  MPU6050_registers();
 
  for (int cal_int = 0; cal_int < 2000 ; cal_int ++){                  //Run this code 2000 times
    if(cal_int % 125 == 0)Serial.print("|");                              //Print a dot on the LCD every 125 readings
    MPU6050_data();                                              //Read the raw acc and gyro data from the MPU-6050
    gyroZerror += gyroZraw;                                              //Add the gyro z-axis offset to the gyro_z_cal variable
    delay(3);                                                          //Delay 3us to simulate the 250Hz program loop
  }
  gyroZerror /= 2000;    
 
  Serial.println("");                                             //Divide the gyro_z_cal variable by 2000 to get the avarage offset
  Serial.print("Gyro_Z_average / yawAxis: ");
  Serial.print(gyroZerror);
  Serial.println("");
 
  previousTime = millis();
}
 
//=================================== --------------------------- ===================================
//=================================== --- MPU6050 CALCULATOR ---- ===================================
//=================================== --------------------------- ===================================
 
void MPU6050_calculator(){
  MPU6050_data();
  gyroZcorr = (gyroZraw - gyroZerror) / 65.5;
 
  currentTime = millis();
  deltaTime = currentTime - previousTime;
 
  angleZ += gyroZcorr * deltaTime / 1000;
  Serial.print("DeltaTime: ");
  Serial.print(deltaTime);
  Serial.print("  ||  Gyro Raw: ");
  Serial.print(gyroZraw);
  Serial.print("  ||  Gyro Error: ");
  Serial.print(gyroZerror);
  Serial.print("  ||  Gyro Corrected: ");
  Serial.print(gyroZcorr);
  Serial.print("  ||  AngleZ: ");
  Serial.println(angleZ);
 
  previousTime = currentTime;  
}/code]

I want to credit Joppa Broking for his easy guide on how the wire library work and how to get the values from the MPU6050
https://www.youtube.com/user/MACPUFFDOG :slight_smile: