from machine import Pin, ADC, PWM
import time

# ----------------------------
# Pin definitions (Raspberry Pi Pico / RP2040)
# ----------------------------
SERVO1_PIN = 18
POT1_PIN   = 26  # ADC0

SERVO2_PIN = 19
POT2_PIN   = 27  # ADC1

SERVO3_PIN = 20
POT3_PIN   = 28  # ADC2

# Servo pulse width limits (microseconds)
SERVO_MIN_US = 500
SERVO_MAX_US = 2500

# Servo PWM frequency
SERVO_HZ = 50
PERIOD_US = 1_000_000 // SERVO_HZ  # 20,000 us

# ----------------------------
# Helper functions
# ----------------------------
def map_range(x, in_min, in_max, out_min, out_max):
    # Integer linear mapping (Arduino-like map)
    return (x - in_min) * (out_max - out_min) // (in_max - in_min) + out_min

def us_to_duty_u16(us):
    # Convert pulse width in microseconds to duty_u16 (0..65535)
    return (us * 65535) // PERIOD_US

# ----------------------------
# ADC setup
# ----------------------------
pot1 = ADC(POT1_PIN)
pot2 = ADC(POT2_PIN)
pot3 = ADC(POT3_PIN)

# ----------------------------
# PWM setup for servos
# ----------------------------
servo1 = PWM(Pin(SERVO1_PIN))
servo2 = PWM(Pin(SERVO2_PIN))
servo3 = PWM(Pin(SERVO3_PIN))

servo1.freq(SERVO_HZ)
servo2.freq(SERVO_HZ)
servo3.freq(SERVO_HZ)

# ----------------------------
# Main loop
# ----------------------------
while True:
    # RP2040 ADC gives 0..65535
    adc1 = pot1.read_u16()
    adc2 = pot2.read_u16()
    adc3 = pot3.read_u16()

    # Map ADC (0..65535) to servo pulse width (us)
    us1 = map_range(adc1, 0, 65535, SERVO_MIN_US, SERVO_MAX_US)
    us2 = map_range(adc2, 0, 65535, SERVO_MIN_US, SERVO_MAX_US)
    us3 = map_range(adc3, 0, 65535, SERVO_MIN_US, SERVO_MAX_US)

    # Write PWM duty corresponding to pulse width
    servo1.duty_u16(us_to_duty_u16(us1))
    servo2.duty_u16(us_to_duty_u16(us2))
    servo3.duty_u16(us_to_duty_u16(us3))

    # Debug
    print("Pot_1:", adc1, "-->", us1,
          "| Pot_2:", adc2, "-->", us2,
          "| Pot_3:", adc3, "-->", us3)

    time.sleep_ms(10)