from imu import MPU6050
from time import sleep
from machine import Pin, I2C, PWM

def rangify(value):
    init_start = -250
    init_end = 250

    final_start = 1800
    final_end = 8000

    init_range = init_end - init_start 
    final_range = final_end - final_start 

    ratio = final_range / init_range

    distance = value - init_start 
    new_value = distance * ratio + final_start 

    return int(new_value)

i2c = I2C(1, sda=Pin(26), scl=Pin(27), freq=400000)
imu = MPU6050(i2c)

servo = PWM(Pin(16))
servo.freq(50)

while True:
    gy=round(imu.gyro.y)
    duty = int(rangify(gy))
    servo.duty_u16(duty)

    print("Servo Duty:", duty, "\t Gyroscopic Y Rotation Rate:", gy)