Code
#include <Wire.h>
#include <Servo.h>
// Define MPU6050 I2C address
#define MPU6050_ADDRESS 0x68
// MPU6050 register addresses
#define MPU6050_REG_ACCEL_XOUT_H 0x3B
#define MPU6050_REG_PWR_MGMT_1 0x6B
#define MPU6050_REG_GYRO_CONFIG 0x1B
// Servo motor pin
#define SERVO_PIN 13
// Variables for gyroscope readings
int16_t gyroX, gyroY, gyroZ;
float rollAngle; // roll angle in degrees
// Initialize Servo object
Servo myservo;
void setup() {
// Start I2C communication
Wire.begin();
// Initialize Serial communication
Serial.begin(9600);
// Wake up MPU6050
Wire.beginTransmission(MPU6050_ADDRESS);
Wire.write(MPU6050_REG_PWR_MGMT_1);
Wire.write(0);
Wire.endTransmission(true);
// Configure gyroscope range
Wire.beginTransmission(MPU6050_ADDRESS);
Wire.write(MPU6050_REG_GYRO_CONFIG);
Wire.write(0x08); // set gyro range to +/-500 degrees per second
Wire.endTransmission(true);
// Attach servo motor to SERVO_PIN
myservo.attach(SERVO_PIN);
}
void loop() {
// Read gyroscope data
Wire.beginTransmission(MPU6050_ADDRESS);
Wire.write(MPU6050_REG_ACCEL_XOUT_H);
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_ADDRESS, 6, true);
gyroX = Wire.read() << 8 | Wire.read();
gyroY = Wire.read() << 8 | Wire.read();
gyroZ = Wire.read() << 8 | Wire.read();
// Calculate roll angle
rollAngle = atan2(gyroY, sqrt(pow(gyroX, 2) + pow(gyroZ, 2))) * 180 / PI;
// Map roll angle to servo motor position
int servoPos = map(rollAngle, -90, 90, 0, 180);
// Move servo motor to desired position
myservo.write(servoPos);
// Print roll angle to Serial monitor
Serial.print("Roll Angle: ");
Serial.print(rollAngle);
Serial.print(" degrees");
// Delay for stability
delay(100);
}