import digitalio
import board, time, pwmio, adafruit_mpr121
from adafruit_motor import servo

pwm = pwmio.PWMOut(board.GP14, frequency=50)
servo1 = servo.Servo(pwm, max_pulse=2500)

pwm2 = pwmio.PWMOut(board.GP15, frequency=50)
servo2 = servo.Servo(pwm2, max_pulse=2500)

# Define UP and DOWN angles
UP_ANGLE = 100      # horizontal player position
DOWN_ANGLE = 0   # kicking position

# Setup i2c on the Pico w/STEMMA_QT wired as shown in the diagram - GP4 (SDA/Blue), GP5 (SCL/Yellow, Power 3.3v(Out) Red
i2c = board.STEMMA_I2C() # Same for any board w/built-in STEMMA_QT port
# i2c = board.I2C() # for the CircuitPlayground boards
touch_pad = adafruit_mpr121.MPR121(i2c)

# Initialize servos to UP position
servo1.angle = UP_ANGLE
servo2.angle = UP_ANGLE

print("Foosball controller ready (CircuitPython)")

# Track previous button states
last_state1 = None
last_state2 = None

try:
    while True:
        if touch_pad[0].value:
            servo1.angle = DOWN_ANGLE
        else:
            servo1.angle = UP_ANGLE

        if touch_pad[1].value:
            servo2.angle = DOWN_ANGLE
        else:
            servo2.angle = UP_ANGLE


        time.sleep(0.01)
except KeyboardInterrupt:
    print("Shutting down...")
    servo1.angle = None
    servo2.angle = None