//Interface MPU6050 Accelerometer and Gyroscope to Arduino Uno and display accelerometer data
//on Serial Monitor

/*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 AccelX, AccelY, AccelZ = 0;
float accelXangle, accelYangle = 0;
float pitch, roll = 0;

void setup() {
  Serial.begin(115200);       //set baud rate to 115200 for serial transmission
  Wire.begin();
  resetMPU();                 //reset MPU to default settings
  setAccelSensitivity(0x00);  //programmable range of +/-2g, +/-4g, +/-8g, +/-16g
  delay(10000);               //wait for MPU6050 to stabilize
}

void loop() {
  readAccel(16384.0);     //read XYZ Accel data from registers 0x3B to 0x40
  displaySerial();        //display yaw, pitch, and roll on Serial Monitor
  delay(10);

}

void displaySerial(void){
  Serial.print(AccelX);
  Serial.print("\t");
  Serial.print(AccelY);
  Serial.print("\t");
  Serial.print(AccelZ);
  Serial.println("\t");

  Serial.print(" Pitch: ");
  Serial.print(accelYangle);
  Serial.print(" Roll: ");
  Serial.println(accelXangle);

}

void readAccel(float accelDivisor){
  //NOTE: as you increase the accelerometer's range, the resolution decreases
  //+/-2g, use divisor of 16384 (14-bit resolution)
  //+/-4g, use divisor of 8192 (13-bit resolution)
  //+/-8g, use divisor of 4096 (12-bit resolution)
  //+/-16g, use divisor of 2048 (11-bit resolution)

  Wire.beginTransmission(MPUaddr0);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(MPUaddr0, 6);    //read 6 consecutive registers starting at 0x3B
  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
    AccelX = (float) (temp0 | temp1);
    AccelX = AccelX / accelDivisor;

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

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

  }
  //You can only calculate roll and pitch from accelerometer data
  accelXangle = (atan2(AccelY, AccelZ)) * 180 / PI;                                   //calculate roll
  accelYangle = (atan2(-AccelX, sqrt(pow(AccelY, 2) + pow(AccelZ, 2)))) * 180 / PI;   //calculate pitch

}

void setAccelSensitivity(uint8_t g){
  //Config AFS_SEL[1:0] bits 4 and 3 in register 0x1C
  //0x00: +/-2g (default)
  //0x08: +/-4g
  //0x10: +/-8g
  //0x18: +/-16g

  Wire.beginTransmission(MPUaddr0);   //initialize comm with MPU @ 0x68
  Wire.write(0x1C);                   //write to register 0x1C
  Wire.write(g);                      //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);
}