# servo_balancer.py
# Uses a hobby servo to balance a marble on a tilting track using photosensor feedback.
#
# This assumes a tiny 9G servo has been wired up to the Pico as follows:
#   Pico pin 40 (VBUS)  -> servo red   (+5V)
#   Pico pin 38 (GND)   -> servo brown (GND)
#   Pico pin 1  (GP0)   -> servo orange (SIG)
#
# This assumes a LTH-1550 photoreflective sensor is connected to digital input
# GP15 using an external 22K pullup to 3.3V on GP15 and the collector lead of the
# phototransistor.  The sensor is active-low: the input is false when an object
# is present.
#   Pico pin 20 (GP15)  <- photoreflective sensor

################################################################################
# load standard Python modules
import math, time

# load the CircuitPython hardware definition module for pin definitions
import board

# load the module for the sensor input
from digitalio import DigitalInOut, Direction, Pull

# load the servo.py module located in same folder
import servo

#--------------------------------------------------------------------------------
# Set up a digital sensor input on GP15, which is physically pin 20 at one corner of the board.
# E.g., this may be attached photointerrupter with associated pullup resistor.
sensor = DigitalInOut(board.GP15)
sensor.direction = Direction.INPUT

# Block until an object is present at the sensor.
def wait_until_present():
    while True:
        if sensor.value is False:
            print("detected object")
            return
        time.sleep(0.01)

#--------------------------------------------------------------------------------
# Set up the servo object on GP0.
actuator = servo.Servo(pin=board.GP0)

#--------------------------------------------------------------------------------
# Calibration constants empirically tuned for the specific device.
center = 78
half   = 10
delay  = 0.1

print("moving to center")
actuator.write(center)
time.sleep(1.0)

#--------------------------------------------------------------------------------
# Begin the main processing loop.
while True:

    print("moving to low position")
    actuator.write(center-half)
    wait_until_present()
    time.sleep(delay)

    print("moving to high position")
    actuator.write(center+half)
    wait_until_present()
    time.sleep(delay)