/* Get all possible data from MPU6050
* Accelerometer values are given as multiple of the gravity [1g = 9.81 m/s²]
* Gyro values are given in deg/s
* Angles are given in degrees
* Note that X and Y are tilt angles and not pitch/roll.
*
* License: MIT
*/
#include "Wire.h"
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
unsigned long timer = 0;
void setup() {
Serial.begin(9600);
Wire.begin();
byte status = mpu.begin();
Serial.print(F("MPU6050 status: "));
Serial.println(status);
while(status!=0){ } // stop everything if could not connect to MPU6050
Serial.println(F("Calculating offsets, do not move MPU6050"));
delay(1000);
mpu.calcOffsets(true,true); // gyro and accelero
Serial.println("Done!\n");
}
void loop() {
mpu.update();
if(millis() - timer > 1000){ // print data every second
float xAcc = mpu.getAccX();
float yAcc = mpu.getAccY();
float zAcc = mpu.getAccZ();
float rollRad = mpu.getAngleX() * M_PI / 180;
float pitchRad = mpu.getAngleY() * M_PI / 180;
float yawRad = mpu.getAngleZ() * M_PI / 180;
Serial.print(F("ACCELERO X: "));Serial.print(xAcc);
Serial.print("\tY: ");Serial.print(yAcc);
Serial.print("\tZ: ");Serial.println(zAcc);
float x = xAcc, y = yAcc, z = 0;
float xAccNew = xAcc, yAccNew = yAcc, zAccNew = zAcc;
xAccNew -= x * cos(yawRad) - y * sin(yawRad) ;
yAccNew -= x * sin(yawRad) - y * cos(yawRad) ;
x = xAccNew, z = zAccNew;
xAccNew -= x * cos(pitchRad) + z * sin(pitchRad) ;
zAccNew -= - x * sin(pitchRad) + z * cos(pitchRad) ;
y = yAccNew, z = zAccNew;
yAccNew -= y * cos(rollRad) - z * sin(rollRad);
zAccNew -= y * sin(rollRad) + z * cos(rollRad);
Serial.print(F("ACCELERO X: "));Serial.print(xAccNew);
Serial.print("\tY: ");Serial.print(yAccNew);
Serial.print("\tZ: ");Serial.println(zAccNew);
Serial.print(F("ANGLE X: "));Serial.print(rollRad);
Serial.print("\tY: ");Serial.print(pitchRad);
Serial.print("\tZ: ");Serial.println(yawRad);
Serial.println(F("=====================================================\n"));
timer = millis();
//Serial.print(F("TEMPERATURE: "));Serial.println(mpu.getTemp());
/*Serial.print(F("GYRO X: "));Serial.print(mpu.getGyroX());
Serial.print("\tY: ");Serial.print(mpu.getGyroY());
Serial.print("\tZ: ");Serial.println(mpu.getGyroZ());
Serial.print(F("ACC ANGLE X: "));Serial.print(mpu.getAccAngleX());
Serial.print("\tY: ");Serial.println(mpu.getAccAngleY());*/
}
}