/*
   Gyroscope Sampling Reference Code V1

   Use the following to get accustomed operating and sampling the GY-521 MPU6050 IMU using the
   ELEGOO Car Kit.
   Ensure that the GY-521 MPU6050 IMU module is properly connected to the Arduino microsontroller.
   You may use this code to have a stronger understanding of the sampling of the IMU.

   The following will rotate the robot 90 degrees in the counterclockwise direction and -90 degrees
   in the clockwise direction using the gyroscope as such only the Z axis of the gyroscope is necessary

   Require Components:
      - ELEGOO Car Kit
      - USB-A to USB-B cable (Arduino USB cable)
      - GY-521 MPU6050 IMU module
      - Wires
      - Breadboard

   Pinout:
      - SDA pin of MPU6050 --> Pin 20
      - SCL pin of MPU6050 --> Pin 21

   NOTE: The Arduino UNO and the Arduino MEGA 2560 have 2 SDA and SCL pins
      - Both have an SDA and SCL pin near the AREF pin
      - UNO has an additional SDA at pin A4 and SCL at pin A5
      - MEGA 2560 has an additional SDA at pin 20 and SCL at pin 21
*/

// I2Cdev and MPU6050 must be installed as libraries in Arduino
// Libraries required for the MPU6050
#include <Wire.h>                            // Wire library require for I2C protocol
#include <I2Cdev.h>                          // Include I2C protocol
#include <MPU6050.h>                         // Include MPU-6050 sensor

// Uncomment the method you would like to test (NOTE: DEGREE_METHOD_USING_MICROS is the most accurate)
//------------------------------------------------------------------------------------------------
//#define DEGREE_METHOD_USING_MILLIS
#define DEGREE_METHOD_USING_MICROS
//#define DEGREE_METHOD_USING_SAMPLE_RATE
//------------------------------------------------------------------------------------------------

MPU6050 gyroAccelTemp;                       // Declare a MPU6050 object instance

//Define the properties of the MPU-6050 (IMU)
float gyroDegree;
/*If you have any Serial commands at baud rate < 98400, then the above value has to decrease*/
#define GYRO_Z_OFFSET 16                     // Determined using IMU_Zero under File > Examples > MPU6050

#ifdef DEGREE_METHOD_USING_MILLIS
  bool isFirstLoopComplete;                  // Declaring the isFirstLoopComplete boolean flag
  float previousTime;                        // Declaring the value to hold the time
#endif

#ifdef DEGREE_METHOD_USING_MICROS
  bool isFirstLoopComplete;                  // Declaring the isFirstLoopComplete boolean flag
  float previousTime;                        // Declaring the value to hold the time
#endif

#ifdef DEGREE_METHOD_USING_SAMPLE_RATE
  #define SAMPLE_RATE 1667                   // This is the default sampling rate in Hz of the MPU 6050
#endif

void setup() {
  Serial.begin(115200);                      // Initialize Serial Communication

  // Initialize the IMU
  Wire.begin();
  Serial.println("Initializing I2C devices...");
  gyroAccelTemp.initialize();

  // Verify IMU connection
  Serial.println("Testing device connections...");
  Serial.println(gyroAccelTemp.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  gyroAccelTemp.CalibrateGyro();                  // Fully calibrate gyro in about 15 loops
  Serial.println("Calibration complete");         // Notify when calibration is complete
  /* The following will NOT get rid of the noise produce by the IMU but they will ensure that
      the average values are at 0 to a certain extent
      (Refer to the IMU Error Determination code and the MPU6050 tutorial video for more details)
  */
  // Set Gyroscope Offsets
  gyroAccelTemp.setZGyroOffset(GYRO_Z_OFFSET);    // Set the Z gyroscope offset

  // IMPORTANT: If you do not calibrate after setting your offset, there will be error
  gyroAccelTemp.CalibrateGyro(6);                 // Fine Tuning purposes of gyroscope
  Serial.println("Fine Tuning Complete");         // Notify when fine tuning is complete

  #ifdef DEGREE_METHOD_USING_MILLIS
    isFirstLoopComplete = false;
  #endif

  #ifdef DEGREE_METHOD_USING_MICROS
    isFirstLoopComplete = false;
  #endif
}

void loop() {
  // Get the degree/s data from the Gyroscope
  /*
     According to the MPU 6050 datasheet, the raw value of the gyroscope should be divided
     by 131.0 to output rotation in units of degrees/s
        - The 131.0 accounts for the conversion of the raw data to proper units
     Resolution: +- 250 degrees/s
  */
  float gyroZ = gyroAccelTemp.getRotationZ() / 131.0;         // Get current Z axis orientation in degress per second

  #ifdef DEGREE_METHOD_USING_MILLIS
    /*
       The data extracted from the gyroscope is in degrees per second.
       The time taken to complete one loop is determined using millis()
       NOTE: The first value should not be considered as it is not possible
              to determine the time to complete one loop so a boolean flag is
              used
       NOTE_2: Millis() overflows after about 50 days

       1. Determine time taken to complete one loop
       2. Multiply that value by the gyroscope value extracted from the IMU
       3. Apply proper unit analysis
            --> millis() is in milliseconds (there are 1000 ms in 1 s)
            --> Data from gyroscope is in degree/s
     */
     if (isFirstLoopComplete) {
      float timeForOneLoop = millis() - previousTime;
      //Serial.println(timeForOneLoop);
      gyroDegree += gyroZ * timeForOneLoop / 1000.0;
     }
     // NOTE: Try and keep the following code close to the above if statement
     previousTime = millis();
     
     // Change the boolean flag to true to enable collection of gyroscope data
     if (!isFirstLoopComplete) {
      isFirstLoopComplete = true;
     }
  #endif

  #ifdef DEGREE_METHOD_USING_MICROS
  /*
     This is identical to the method using millis() but uses micros().
     It is more accurrate
     NOTE: micros() overflows after about 70 minutes

     1. Determine time taken to complete one loop
       2. Multiply that value by the gyroscope value extracted from the IMU
       3. Apply proper unit analysis
            --> micros() is in microseconds (there are 1000000 us in 1 s)
            --> Data from gyroscope is in degree/s
   */
  if (isFirstLoopComplete) {
      float timeForOneLoop = micros() - previousTime;
      //Serial.println(timeForOneLoop);
      gyroDegree += gyroZ * timeForOneLoop / 1000000.0;
     }
     // NOTE: Try and keep the following code close to the above if statement
     previousTime = micros();
     
     // Change the boolean flag to true to enable collection of gyroscope data
     if (!isFirstLoopComplete) {
      isFirstLoopComplete = true;
     }
  #endif
  
  #ifdef DEGREE_METHOD_USING_SAMPLE_RATE
    /*
       The data extracted from the gyroscope is in degrees per second.
       The Gyroscope sample rate is in units of Hz, which is second^-1.
          --> The sample rate is a predefined value
       1. Therefore, divide the data extracted by the gyroscope with the sample rate to get degrees
       2. Accumulate the gyroDegree calculated
    */
    gyroDegree += gyroZ / SAMPLE_RATE;
  #endif

  Serial.println(gyroDegree);
}