//Interface MPU6050 Accelerometer and Gyroscope to Arduino Uno and display
//gyroscope data on the Serial Monitor. This example is intended to show
//the drift caused by the approximated integration of the angular velocity.

/*Copyright (c) 2019, ProteShea LLC
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
3. Neither the name of the copyright holders nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <Wire.h>

#define MPUaddr0 0x68    //addr used if pin AD0 is set to 0 (left unconnected)
#define MPUaddr1 0x69    //addr used if pin AD0 is set to 1 (wired to VDD)

float GyroX, GyroY, GyroZ = 0;
float gyroXangle, gyroYangle, gyroZangle = 0;
float yaw, pitch, roll = 0;
float lastTime, realTime, Ts = 0;

void setup() {
  Serial.begin(115200);        //set baud rate to 19200 for serial transmission
  Wire.begin();
  resetMPU();                 //reset MPU to default settings
  setGyroSensitivity(0x00);   //programmable range of +/-250, +/-500, +/-1000, +/-2000 DPS
  delay(10000);               //wait for the device to stabilize
}

void loop() {
  readGyro(131.0);        //read XYZ Gyro data from registers 0x43 to 0x48
  displaySerial();        //display yaw, pitch, and roll on Serial Monitor
  delay(10);

}


void displaySerial(void){
  Serial.print("Yaw: ");
  Serial.print(gyroZangle);
  Serial.print(" Pitch: ");
  Serial.print(gyroYangle);
  Serial.print(" Roll: ");
  Serial.println(gyroXangle);

}


void readGyro(float gyroDivisor){
  // +/-250 dps, use divisor of 131
  // +/-500 dps, use divisor of 65.5
  // +/- 1000 dps, use divisor of 32.8
  // +/- 2000 dps, use divisor of 16.4

  //Time calculations required since Gyro data returned with units of degrees/sec and we just need degrees
  //Have to calculate time elapsed between successive reads of Gyro data
  lastTime = realTime;
  realTime = millis();
  Ts = (realTime - lastTime) / 1000;

  Wire.beginTransmission(MPUaddr0);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(MPUaddr0, 6);    //read 6 consecutive registers starting at 0x43
  if (Wire.available() >= 6){

    int16_t temp0 = Wire.read() << 8;   //read upper byte of X
    int16_t temp1 = Wire.read();        //read lower byte of X
    GyroX = (float) (temp0 | temp1);
    GyroX = GyroX / gyroDivisor;

    temp0 = Wire.read() << 8;           //read upper byte of Y
    temp1 = Wire.read();                //read lower byte of Y
    GyroY = (float) (temp0 | temp1);
    GyroY = GyroY / gyroDivisor;

    temp0 = Wire.read() << 8;           //read upper byte of Z
    temp1 = Wire.read();                //read lower byte of Z
    GyroZ = (float) (temp0 | temp1);
    GyroZ = GyroZ / gyroDivisor;

  }
  //Gyro data is given as angular velocity (degrees/sec) so to get position (degrees)
  //we have to integrate the angular velocity which is approximated by taking the sum
  //of a finite number of samples (2 in this case) taken at an interval Ts
  gyroXangle += GyroX * Ts;
  gyroYangle += GyroY * Ts;
  gyroZangle += GyroZ * Ts;

}


void setGyroSensitivity(uint8_t dps){
  //Config FS_SEL[1:0] bits 4 and 3 in register 0x1B
  //0x00: +/-250 dps (default)
  //0x08: +/-500 dps
  //0x10: +/-1000 dps
  //0x18: +/-2000 dps

  Wire.beginTransmission(MPUaddr0);   //initialize comm with MPU @ 0x68
  Wire.write(0x1B);                   //write to register 0x1B
  Wire.write(dps);                    //setting bit 7 to 1 resets all internal registers to default values
  Wire.endTransmission();             //end comm
}

void resetMPU(void){
  Wire.beginTransmission(MPUaddr0);   //initialize comm with MPU @ 0x68
  Wire.write(0x6B);                   //write to register 0x6B
  Wire.write(0x00);                   //reset all internal registers to default values
  Wire.endTransmission();             //end comm
  delay(100);
}