import machine
import time
from mpu6050 import MPU6050
from machine import Pin, PWM

# Constants
I2C_SCL_PIN = 22  # SCL Pin (GPIO22)
I2C_SDA_PIN = 21  # SDA Pin (GPIO21)
SERVO_PIN_PITCH = 14  # Servo controlling the pitch (GPIO14)
SERVO_PIN_YAW = 15    # Servo controlling the yaw (GPIO15)

# Initialize I2C
i2c = machine.I2C(0, scl=Pin(I2C_SCL_PIN), sda=Pin(I2C_SDA_PIN), freq=400000)

# Initialize MPU6050 sensor
mpu = MPU6050(i2c)

# Initialize servo motors
pitch_servo = PWM(Pin(SERVO_PIN_PITCH), freq=50)
yaw_servo = PWM(Pin(SERVO_PIN_YAW), freq=50)

# Servo calibration (min and max pulse width in microseconds)
SERVO_MIN = 40  # Minimum pulse width
SERVO_MAX = 115  # Maximum pulse width

# Function to map sensor data to servo angle
def map_value(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

# Function to set servo angle (0 to 180 degrees)
def set_servo_angle(servo, angle):
    pulse_width = map_value(angle, 0, 180, SERVO_MIN, SERVO_MAX)
    servo.duty(pulse_width)

# Main loop
while True:
    # Read the accelerometer data (pitch and roll)
    accel_data = mpu.get_accel_data()
    accel_x = accel_data['x']
    accel_y = accel_data['y']
    accel_z = accel_data['z']
    
    # Calculate pitch and yaw (basic sensor fusion can be added here)
    pitch = math.atan2(accel_y, accel_z) * 180 / math.pi  # in degrees
    yaw = math.atan2(accel_x, accel_z) * 180 / math.pi  # in degrees
    
    # Map the pitch and yaw to servo angles
    pitch_angle = map_value(pitch, -90, 90, 0, 180)
    yaw_angle = map_value(yaw, -90, 90, 0, 180)
    
    # Set the servo angles to control the gimbal
    set_servo_angle(pitch_servo, pitch_angle)
    set_servo_angle(yaw_servo, yaw_angle)
    
    # Wait a short time to allow the motors to move
    time.sleep(0.05)