mpu6050

mpu.ino

  // Basic demo for accelerometer readings from Adafruit MPU6050

  // ESP32 Guide: https://RandomNerdTutorials.com/esp32-mpu-6050-accelerometer-gyroscope-arduino/
  // ESP8266 Guide: https://RandomNerdTutorials.com/esp8266-nodemcu-mpu-6050-accelerometer-gyroscope-arduino/
  // Arduino Guide: https://RandomNerdTutorials.com/arduino-mpu-6050-accelerometer-gyroscope/

  #include <Adafruit_MPU6050.h>
  #include <Adafruit_Sensor.h>
  #include <Wire.h>

  Adafruit_MPU6050 mpu;
  sensors_event_t a, g,temp;
  float AccX, AccY, AccZ;
  float GyroX, GyroY, GyroZ;

  float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
  float rolla, pitcha, yawa;

  float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;

  float elapsedTime, currentTime, previousTime;
  void resetGyroAngles() {
  mpu.getEvent(&a, &g,&temp);
    pitcha  = (atan(a.acceleration.y / sqrt(pow(a.acceleration.x, 2) + pow(AccZ, 2))) * 180 / PI);
    rolla  = (atan(-1 * a.acceleration.x / sqrt(pow(a.acceleration.y, 2) + pow(AccZ, 2))) * 180 / PI);
  yawa = 0;
  }
  void IMU_calib()
  { 
      
    
    
    int c=0;
    // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
    // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
    // Read accelerometer values 200 times
    AccErrorX = 0;
    AccErrorY = 0;
    while (c < 3000)
    {
      mpu.getEvent(&a, &g,&temp);
      
      AccX = (a.acceleration.x);
      AccY = (a.acceleration.y);
      AccZ = (a.acceleration.z);
      // Sum all readings
      AccErrorX += (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI)     ;  // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
    AccErrorY += (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) ; // AccErrorY ~(-1.58)
      c++;
    }
    
    //Divide the sum by 200 to get the error value
    AccErrorX /= 3000;
    AccErrorY /= 3000;
    
    c = 0;
    GyroErrorX = 0;
    GyroErrorY = 0;
    GyroErrorZ = 0;
    // Read gyro values 200 times
    while (c < 3000)
    {
      mpu.getEvent(&a, &g,&temp);
      
      GyroX = g.gyro.x;
      GyroY = g.gyro.y;
      GyroZ = g.gyro.z;
      
      // Sum all readings
      GyroErrorX = GyroErrorX + (GyroX);
      GyroErrorY = GyroErrorY + (GyroY);
      GyroErrorZ = GyroErrorZ + (GyroZ);
      
      c++;
      
    }
    
    //Divide the sum by 200 to get the error value
    GyroErrorX /= 3000;
    GyroErrorY /= 3000;
    GyroErrorZ /= 3000;
    
    Serial.println("imu calibrated");
  vTaskDelay(100/portTICK_PERIOD_MS );
  }
  void setup(void) {
    Serial.begin(115200);
    while (!Serial)
      delay(10); // will pause Zero, Leonardo, etc until serial console opens

    Serial.println("Adafruit MPU6050 test!");

    // Try to initialize!
    if (!mpu.begin()) {
      Serial.println("Failed to find MPU6050 chip");
      while (1) {
        delay(10);
      }
    }
    Serial.println("MPU6050 Found!");

    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    Serial.print("Accelerometer range set to: ");
    switch (mpu.getAccelerometerRange()) {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
    }
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    Serial.print("Gyro range set to: ");
    switch (mpu.getGyroRange()) {
    case MPU6050_RANGE_250_DEG:
      Serial.println("+- 250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+- 500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+- 1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+- 2000 deg/s");
      break;
    }

    mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
    Serial.print("Filter bandwidth set to: ");
    switch (mpu.getFilterBandwidth()) {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 Hz");
      break;
    }

    Serial.println("");
    delay(100);
    IMU_calib();
    resetGyroAngles();
  }

  void loop() {
    /* Get new sensor events with the readings */
    
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
  // Divide by 1000 to get seconds
    /* Print out the values */
      GyroX = g.gyro.x - GyroErrorX;
      GyroY = g.gyro.y - GyroErrorY;
      GyroZ = g.gyro.z - GyroErrorZ; 

    AccX = (a.acceleration.x);
      AccY = (a.acceleration.y);
      AccZ = (a.acceleration.z);
    accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI)  -AccErrorX;  // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
    accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) -AccErrorY; // AccErrorY ~(-1.58)


  previousTime = currentTime;        // Previous time is stored before the actual time read
    currentTime = millis();            // Current time actual time read
    elapsedTime = (currentTime - previousTime) / 1000;


  pitcha = 0.8 * (pitcha  + ((GyroX * elapsedTime*180)/PI)) + 0.2 * accAngleX ;
  rolla = 0.8 * (rolla + ((GyroY * elapsedTime*180)/PI)) + 0.2 * accAngleY;
  yawa = yawa + (GyroZ * elapsedTime *180)/PI;

  // Serial.println(GyroErrorZ);
    Serial.print("Rotation X: ");
    Serial.print(pitcha); 
    Serial.print(", Y: ");
    Serial.print(rolla);
    Serial.print(", Z: ");
    Serial.print(yawa);
    Serial.println(" rad");

  //   Serial.println("");
  }