from machine import Pin, PWM, I2C
import utime
from pico_i2c_lcd import I2cLcd


# Initialize I2C for LCD
i2c = I2C(0, sda=Pin(16), scl=Pin(17), freq=400000)

# Initialize Servos (PWM)
servos = [PWM(Pin(pin)) for pin in [2, 3, 4, 5]]
for servo in servos:
    servo.freq(50)  # Standard servo frequency

# Function to move servo (simulate rafter opening)
def move_servo(servo):
    servo.duty_u16(8000)  # Move to open position
    utime.sleep(0.5)
    servo.duty_u16(2000)  # Move back (optional)

# Countdown sequence
for count in range(9, -1, -1):
    lcd.clear()
    lcd.putstr(f"Countdown: {count}")
    
    # Open a rafter every 2 seconds
    if count in [8, 6, 4, 2]:  # Open one rafter at a time
        index = (8 - count) // 2
        move_servo(servos[index])

    utime.sleep(1)

# Display TAKE OFF when countdown reaches 0
lcd.clear()
lcd.putstr("TAKE OFF")