from machine import Pin, PWM
import time

# Define Servo pin
servo_pin = Pin(5)  # Pin for the servo
servo = PWM(servo_pin)
servo.freq(50)  # Standard frequency for servos is 50Hz

# Define Stepper Motor pins
step_pin = Pin(3, Pin.OUT)   # Step pin
dir_pin = Pin(2, Pin.OUT)    # Direction pin

# Variables for controlling the stepper
step_delay = 0.1  # Default step delay
direction = 1     # Default direction (1 = forward, 0 = backward)
last_step_time = time.time()  # Initialize last step time
sweeping = True  # State variable to track servo sweeping direction
current_angle = 0  # Current angle for the servo

def set_angle(angle):
    if angle < 0 or angle > 180:
        print("Angle out of bounds!")
        return
    
    duty = int((angle / 180) * (2600 - 500) + 500)
    servo.duty_u16(duty * 65535 // 20000)
    print(f"Setting servo angle: {angle}, Duty: {duty}")

def set_stepper_direction(direction):
    dir_pin.value(direction)  # Set direction: 0 or 1

def step_stepper():
    step_pin.on()  # Trigger the step pin
    time.sleep(0.01)  # Short pulse duration
    step_pin.off()  # Reset the step pin

def control_stepper():
    global last_step_time, step_delay
    current_time = time.time()
    
    pwm_value = servo.duty_u16()  # Read the PWM value from the servo pin
    print(f"Servo PWM Value: {pwm_value}")  # Debug: Print the PWM value

    # Convert the PWM value to a delay
    if pwm_value > 0:
        # Map the PWM value to a smaller step delay
        step_delay = max(0.01, (65535 - pwm_value) / 65535.0 * 0.1)  # Adjust to achieve faster stepping
        direction = 1 if pwm_value > 32768 else 0  # Change direction based on PWM
        set_stepper_direction(direction)
        
        # Check if it's time to step the motor
        if current_time - last_step_time >= step_delay:
            last_step_time = current_time  # Update last step time
            step_stepper()  # Step the motor
    else:
        print("PWM signal is too low, no stepping.")  # Debug: No stepping

def sweep_servo():
    global current_angle, sweeping

    if sweeping:
        current_angle += 1
        if current_angle > 180:
            current_angle = 180
            sweeping = False  # Change direction
    else:
        current_angle -= 1
        if current_angle < 0:
            current_angle = 0
            sweeping = True  # Change direction

    set_angle(current_angle)

try:
    while True:
        # Sweep the servo and control the stepper in the main loop
        sweep_servo()
        control_stepper()
        time.sleep(0.01)  # Small delay to avoid busy waiting

except KeyboardInterrupt:
    step_pin.off()  # Ensure step pin is low
    servo.duty_u16(0)  # Stop the servo
    print("Program stopped")