import pigpio
import time

SERVO_PIN = 12  # GPIO pin connected to the servo

# Initialize pigpio
pi = pigpio.pi()
if not pi.connected:
    print("Error: Could not connect to pigpio daemon.")
    exit()

def set_speed(speed):
    """Control speed and direction of a continuous rotation servo."""
    if speed > 0:      # Clockwise
        duty_cycle = 1600  # Adjust as needed (1500 = stop, 1600+ = CW)
    elif speed < 0:    # Counterclockwise
        duty_cycle = 1400  # Adjust as needed (1500 = stop, 1400- = CCW)
    else:              # Stop
        duty_cycle = 1500  # Neutral position

    pi.set_servo_pulsewidth(SERVO_PIN, duty_cycle)
    time.sleep(1)  # Allow servo to react

try:
    print("Testing continuous rotation servo...")
    
    print("Rotating Clockwise...")
    set_speed(1)
    time.sleep(2)

    print("Stopping...")
    set_speed(0)
    time.sleep(1)

    print("Rotating Counterclockwise...")
    set_speed(-1)
    time.sleep(2)

    print("Stopping...")
    set_speed(0)

except KeyboardInterrupt:
    print("Test interrupted.")

finally:
    pi.set_servo_pulsewidth(SERVO_PIN, 0)  # Stop servo
    pi.stop()