from machine import Pin, PWM

# Pin definitions: [Left-Front_A, Left-Front_B, Left-Rear_A, Left-Rear_B, 
#                  Right-Front_A, Right-Front_B, Right-Rear_A, Right-Rear_B, 
#                  Cover_A, Cover_B]
servo_pins = [32, 33, 25, 26, 27, 14, 12, 13, 22, 23]

# Initialize PWM objects for all servos at 50Hz
servos = [PWM(Pin(pin), freq=50) for pin in servo_pins]

# Servo configuration parameters
MIN_DUTY = 27    # Duty cycle for -90 degrees
MAX_DUTY = 127   # Duty cycle for 90 degrees
FREQ = 50        # Standard servo frequency (50Hz)

# Calibration offsets for each servo to account for mechanical mounting errors
offsets = [-10, 0, 0, 0, 0, 0, 0, 0, 0, 0]

def set_servo_angle(num, angle):
    """
    Set the angle of a specific servo.
    :param num: Index of the servo in the list (0-9)
    :param angle: Target angle in degrees (-90 to 90)
    """
    # Safety clamp: restrict movement to prevent mechanical damage
    angle = max(-45, min(45, angle))
    
    # Calculate duty cycle based on target angle and specific servo offset
    duty = MIN_DUTY + (MAX_DUTY - MIN_DUTY) * (angle + offsets[num] + 90) / 180
    servos[num].duty(int(duty))
    
def servo_init():
    """
    Initial calibration of all servos to the center (0 degree) position.
    """
    from time import sleep
 
    for i in range(len(servos)):
        set_servo_angle(i, 0)
        sleep(0.1) # Short delay to prevent power surges from simultaneous movement
        
    print("Servo initialization complete!")