# SPDX-FileCopyrightText: 2018 Kattni Rembor for Adafruit Industries
#
# SPDX-License-Identifier: MIT

"""CircuitPython Essentials Servo standard servo example"""
import time
import board
import pwmio
from adafruit_motor import servo
from adafruit_circuitplayground import cp

# create a PWMOut object on Pin A2.
# create a PWMOut object on Pin A1.
pwm2 = pwmio.PWMOut(board.A2, duty_cycle=2 ** 15, frequency=50)
pwm1 = pwmio.PWMOut(board.A1, duty_cycle=2 ** 15, frequency=50)

# Create a servo object, my_servo.
my_servo1 = servo.ContinuousServo(pwm1)
my_servo2 = servo.ContinuousServo(pwm2)

cp.pixels.brightness = .3

while True:
    if cp.button_a:
        cp.pixels.fill((3, 252, 240))

        print("forward")
        my_servo1.throttle = -1.0
        my_servo2.throttle = 1.0
        time.sleep(2.0)
        print("Stop")
        my_servo1.throttle = 0.0
        my_servo2.throttle = 0.0
        time.sleep(2.0)

        cp.play_tone(262, 1)
        cp.play_tone(294, 1)

    if cp.button_b:
        cp.pixels.fill((239, 242, 22))

        print("reverse")
        my_servo1.throttle = 1.0
        my_servo2.throttle = -1.0
        time.sleep(2.0)
        print("Stop")
        my_servo1.throttle = 0.0
        my_servo2.throttle = 0.0
        time.sleep(1.0)

        cp.play_tone(250, 1)
        cp.play_tone(375, 1)
        cp.play_tone(450, 1)

    if cp.light > 300:
        cp.pixels.fill((204, 18, 34))

        print("forward")
        my_servo1.throttle = 1.0
        print("reverse")
        my_servo2.throttle = 1.0
        time.sleep(3.0)
        print("Stop")
        my_servo1.throttle = 0.0
        my_servo2.throttle = 0.0
        time.sleep(2.0)