#include <Wire.h>
#define ACCLX
#define ACCLY
#define ACCLZ
#define GYROX
#define GYROY
#define GYROZ
#define ROLL
#define PITCH
#define TEMP
#define YAW
double initialYaw = 0.0; // Initial yaw angle
double currentYaw = initialYaw;
unsigned long previousTime = 0; // Store the previous time
const float g = 9.8;
const float gyroSensitivity = 0.74;
const int MPU6050_addr = 0x68; // I2C address of the MPU-6050
int16_t accelerometer_x, accelerometer_Y, accelerometer_Z;
int16_t gyro_x, gyro_y, gyro_z;
float xlConv(int16_t accl){
float acceleration;
acceleration = (accl / 1000) * g;
return acceleration;
}
void setup() {
Serial.begin(9600);
Wire.begin(); // Initialize I2C
Wire.beginTransmission(MPU6050_addr); // Start communication with MPU6050
Wire.setClock(400000);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // Set to 0 to wake up MPU-6050
Wire.endTransmission(true);
}
void loop() {
/*
6 bytes for accelerometer
2 bytes x
2 bytes y
2 bytes z
2 bytes for temperature
6 bytes for gyroscope
2 bytes x
2 bytes y
2 bytes z
*/
#ifdef ACCLX
Wire.endTransmission(false);
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x3B); // starting with register (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 2, true); // request 2 registers(16 bits in total)
accelerometer_x = Wire.read() << 8 | Wire.read();
Serial.print("Acceleration X (m/s^2) :");
Serial.print(xlConv(accelerometer_x)); Serial.print(", ");
#endif
#ifdef ACCLY
Wire.endTransmission(false);
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x3D); // starting with register (ACCEL_XOUT_Y)
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 2, true);
accelerometer_Y = Wire.read() << 8 | Wire.read();
Serial.print("Acceleration Y (m/s^2) :");
Serial.print(xlConv(accelerometer_Y)); Serial.print(", ");
#endif
#ifdef ACCLZ
Wire.endTransmission(false);
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x3F); // starting with register (ACCEL_XOUT_Z)
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 2, true);
accelerometer_Z = Wire.read() << 8 | Wire.read();
Serial.print("Acceleration Z (m/s^2) :");
Serial.print(xlConv(accelerometer_Z)); Serial.print(", ");
#endif
#ifdef TEMP
Wire.endTransmission(false);
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x41); // starting with register (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 2, true);
int16_t temperature = Wire.read() << 8 | Wire.read(); // This value is rarely used
Serial.print("Temp (°C): ");
Serial.print(temperature / 340.00 + 36.53); Serial.print(", ");
#endif
#ifdef GYROX
Wire.endTransmission(false);
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x43); // starting with register (GYRO_XOUT)
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 2, true);
gyro_x = Wire.read() << 8 | Wire.read();
Serial.print("Gyro X(deg/s): ");
Serial.print(gyro_x); Serial.print(", ");
#endif
#ifdef GYROY
Wire.endTransmission(false);
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x45); // starting with register (GYRO_YOUT)
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 2, true);
gyro_y = Wire.read() << 8 | Wire.read();
Serial.print("Gyro Y(deg/s): ");
Serial.print(gyro_y); Serial.print(", ");
#endif
#ifdef GYROZ
Wire.endTransmission(false);
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x47); // starting with register (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 2, true);
gyro_z = Wire.read() << 8 | Wire.read();
Serial.print("Gyro Z(deg/s): ");
Serial.print(gyro_z); Serial.print(", ");
#endif
#ifdef ROLL
double roll = atan2(accelerometer_Y, accelerometer_Z);
roll = roll * 180.0 / PI;
Serial.print("Roll (degrees): ");
Serial.println(roll);
#endif
#ifdef PITCH
double pitch = atan2(-accelerometer_x, sqrt(accelerometer_Y * accelerometer_Y + accelerometer_Z * accelerometer_Z));
pitch = pitch * 180.0 / PI;
Serial.print("Pitch (degrees): ");
Serial.println(pitch);
#endif
#ifdef YAW
unsigned long currentTime = millis();
double deltaTime = (currentTime - previousTime) / 1000.0; // Convert to seconds
double gyroZ = gyro_z / gyroSensitivity;
double deltaAngle = gyroZ * deltaTime; // Change in yaw angle
currentYaw += deltaAngle;
if (currentYaw > 180.0) {
currentYaw -= 360.0;
} else if (currentYaw < -180.0) {
currentYaw += 360.0;
}
Serial.print("Yaw (degrees): ");
Serial.println(currentYaw);
previousTime = currentTime;
#endif
delay(1000);
}