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

# 1
# =========================================================
# I2C SETUP (MPU6050 connection)
# =========================================================
# Initialize I2C communication with MPU6050 sensor
# SCL -> GPIO22, SDA -> GPIO21

i2c = I2C(0,scl=Pin(22),sda=Pin(21))
MPU_ADDR= 0x68
# 2
# Wake up the MPU6050 (disable sleep mode)
i2c.writeto_mem(MPU_ADDR,0x6B , b'\x00')

# =========================================================
# SERVO SETUP
# =========================================================
# Servo connected to GPIO26 using PWM at 50Hz

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

# 2
# ---------------------------------------------------------
# Function: set_angle()
# ---------------------------------------------------------
# Converts an angle (0–180 degrees) into a PWM signal
# for controlling the servo motor

def set_angle(angle):

    # Limit angle to safe range
    if angle < 0:
        angle = 0
    if angle > 180:
        angle = 180

    # Convert angle to microseconds pulse width
    min_us = 500
    max_us = 2500

    us = min_us + (angle / 180) * (max_us - min_us)

    # Convert microseconds to duty cycle (ESP32 PWM)
    duty = int((us / 20000) * 1023)

    servo.duty(duty)

# 3
# =========================================================
# FUNCTION: READ GYROSCOPE DATA
# =========================================================
# Reads 6 bytes from MPU6050 gyro registers (0x43)
# gx, gy, gz are 16-bit signed values
def read_gyro():
    data= i2c.readfrom_mem(MPU_ADDR,0x43,6)
    gx,gy , gz = struct.unpack(">hhh",data)
    return gx,gy,gz

# =========================================================
# START POSITION (CENTER SERVO)
# =========================================================
set_angle(90)


# 4
# =========================================================
# MAIN LOOP
# =========================================================
while True:

    # Read raw gyroscope values (X, Y, Z axes)
    gx,gy,gz = read_gyro()

    # Convert raw values to degrees per second
    #  -32768  <==> +32767 -> 16 bit
    # 250 degree / sec is the smallest value we can use with this mpu ,
    #  this small value give use high resolution and sensitivity 
    #  131 : represent -> Gyroscope Sensitivity Scale Factor
    gx = gx /131 
    gy = gy/131
    gz=gz/131


    # =====================================================
    # PRINT ALL AXES VALUES
    # =====================================================
    print ("gx  : ", gx,"|gy : ",gy,"|gz:  ",gz)
    # =====================================================
    # STEERING LOGIC (USING Z-AXIS / YAW ROTATION)
    # =====================================================
    # Z-axis represents left/right rotation (steering)
    angle= 90+gz
    # Move servo to calculated angle
    set_angle(angle)
    # Small delay for stability
    time.sleep(0.02)