from machine import Pin, PWM
import utime

# Stepper motor configuration
step = Pin(17)
direction = Pin(16, Pin.OUT)

# Servo configurations
servo1_pwm = PWM(Pin(18), freq=50)  # Servo 1 on Pin 18
servo2_pwm = PWM(Pin(19), freq=50)  # Servo 2 on Pin 19
# Stepper motor setup
@rp2.asm_pio(set_init=rp2.PIO.OUT_LOW)
def move():
    wrap_target()
    set(pins, 1)   [31]
    nop()          [31]
    nop()          [31]
    set(pins, 0)   [31]
    nop()          [31]
    nop()          [31]
    wrap()

motor = rp2.StateMachine(0, move, freq=100000, set_base=step)

def change_direction(direction_input):
    if direction_input == "forward":
        direction.value(0)
        print("Motor direction set to forward")
    elif direction_input == "reverse":
        direction.value(1)
        print("Motor direction set to reverse")
    else:
        print("Invalid direction. Please use 'forward' or 'reverse'.")

# Servo control functions
def set_servo1_angle(angle):
    duty_cycle = int(750 + (angle / 180) * 2500)
    servo1_pwm.duty_u16(duty_cycle)

def set_servo2_angle(angle):
    duty_cycle = int(750 + (angle / 180) * 2500)
    servo2_pwm.duty_u16(duty_cycle)

# Start the motor initially in one direction (e.g., clockwise).
change_direction("forward")
motor.active(1)

while True:
    user_input = input("Enter direction ('forward' or 'reverse'), servo1 angle, servo2 angle, or 'exit' to stop: ")
    
    if user_input == "exit":
        break
    elif user_input.startswith("servo1 "):
        angle = float(user_input.split()[1])
        print(angle)
        set_servo1_angle(angle)
    elif user_input.startswith("servo2 "):
        angle = float(user_input.split()[1])
        print(angle)
        set_servo2_angle(angle)
    else:
        change_direction(user_input)

# Stop the motor and servo PWM signals.
motor.active(0)
servo1_pwm.deinit()
servo2_pwm.deinit()