from machine import Pin, I2C, PWM
import time
import math

class MPU6050:
    def __init__(self, i2c, addr=0x68):
        self.i2c = i2c
        self.addr = addr
        self.i2c.writeto(self.addr, bytes([0x6B, 0])) 

    def read_raw_accel(self):
        data = self.i2c.readfrom_mem(self.addr, 0x3B, 6)
        ax = self._bytes_to_int(data[0], data[1])
        ay = self._bytes_to_int(data[2], data[3])
        az = self._bytes_to_int(data[4], data[5])
        return ax, ay, az

    def _bytes_to_int(self, msb, lsb):
        val = (msb << 8) | lsb
        if val > 32767:
            val -= 65536
        return val

i2c = I2C(0, scl=Pin(22), sda=Pin(21))
mpu = MPU6050(i2c)

servo_x = PWM(Pin(32), freq=50)
servo_y = PWM(Pin(33), freq=50)

def map_value(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

def move_servo(pwm_obj, angle):
    if angle < 0: angle = 0
    if angle > 180: angle = 180
    
    duty = int(map_value(angle, 0, 180, 1638, 8192))
    pwm_obj.duty_u16(duty)

while True:
    ax, ay, az = mpu.read_raw_accel()

    angle_x = map_value(ax, -16384, 16384, 180, 0) 
    angle_y = map_value(ay, -16384, 16384, 180, 0)

    g_x = ax / 16384.0
    g_y = ay / 16384.0

    move_servo(servo_x, angle_x)
    move_servo(servo_y, angle_y)
    
    print("Axis X: {:.2f} g ({}°) | Axis Y: {:.2f} g ({}°)"
          .format(g_x, int(angle_x), g_y, int(angle_y)))

    time.sleep(0.1)