from time import sleep
from machine import PWM, Pin
from micropython_motor import MOTOR

OP_DURATION = 2  # The operation duration in seconds

# Pin Mapping
drv_switch = Pin(11, Pin.OUT)  # Pin to enable/disable the DRV8833
drv8833_ain1 = PWM(Pin(9, Pin.OUT))
drv8833_ain1.freq(50)
drv8833_ain2 = PWM(Pin(10, Pin.OUT))
drv8833_ain2.freq(50)

"""The PWM enabled pin connected to the AIN1 (motor A control 1) pin of the
DRV8833 motor driver board.  Specifying a PWM frequency of less than 100 Hz
typically improves the low speed operation of brushed DC motors.
"""
motor_a = MOTOR(drv8833_ain1, drv8833_ain2)

drv_switch.on()

# Drive backwards at 50% throttle
motor_a.throttle = 1.0
sleep(OP_DURATION)

# Coast to a stop
motor_a.throttle = None
sleep(OP_DURATION)

# Drive backwards at 50% throttle
motor_a.throttle = -0.5
sleep(OP_DURATION)

# Brake to a stop
motor_a.throttle = 0
sleep(OP_DURATION)

drv_switch.off()

import time
from machine import Pin, PWM
from micropython_motor.servo import Servo

servo = PWM(Pin(10, Pin.OUT))
servo.freq(50)

servo7 = Servo(servo)

for i in range(180):
    servo7.angle = i
    time.sleep(0.03)
for i in range(180):
    servo7.angle = 180 - i
    time.sleep(0.03)

# You can also specify the movement fractionally.
fraction = 0.0
while fraction < 1.0:
    servo7.fraction = fraction
    fraction += 0.01
    time.sleep(0.06)

import time
from machine import Pin
from micropython_motor import stepper

print("Stepper test")


mode = -1

# Mode button setup
drv_switch = Pin(16, Pin.OUT)
drv_switch.on()

# Stepper motor setup
DELAY = 0.006  # fastest is ~ 0.004, 0.01 is still very smooth, gets steppy after that
STEPS = 513  # this is a full 360º
coils = (Pin(21, Pin.OUT), Pin(20, Pin.OUT), Pin(19, Pin.OUT), Pin(18, Pin.OUT))


stepper_motor = stepper.StepperMotor(
    coils[0], coils[1], coils[2], coils[3], microsteps=None
)


def stepper_fwd():
    print("stepper forward")
    for _ in range(STEPS):
        stepper_motor.onestep(direction=stepper.FORWARD)
        time.sleep(DELAY)
    stepper_motor.release()


def stepper_back():
    print("stepper backward")
    for _ in range(STEPS):
        stepper_motor.onestep(direction=stepper.BACKWARD)
        time.sleep(DELAY)
    stepper_motor.release()


def run_test(testnum):
    if testnum == 0:
        stepper_fwd()
    elif testnum == 1:
        stepper_back()


while True:
    mode = (mode + 1) % 2
    print("switch to mode %d" % (mode))
    print()
    time.sleep(0.8)  # big debounce
    run_test(mode)