########################################
# TEST FOR BUTTONS
# Controller for a Robot Car.  This will
# - create a WiFi AccessPoint
# - By default, this will have IP Address of 192.168.4.1
# - listen for UDP messages on port
# - receive messages on the port and convert into instructions to two motors
# - Expects to receivemessages continually from the controller  (every 50ms)
########################################
from machine import Pin
from time import sleep  

# Commands that can be received
# Each motor can be fwd, back, stop
CMD_FWD="F"
CMD_BACK="B"
#CMD_LEFT="L"
#CMD_RIGHT="R"
CMD_STOP="S"
CMD_FIRE="X"

DEBUG = True

########################################
# FOR PINOUTS should pin number
# d0 = 16  d1=05  d2=04  d3=0 d4=02
# d5=14  d6=12  d7=13  d8=15
########################################
# Motor A - select appropriate pin numbers - One pin for forward, one for back
motor1Pin1 = Pin(15, Pin.OUT)
motor1Pin2 = Pin(13, Pin.OUT)
# Motor B - select appropriate pin numbers - One pin for forward, one for back
motor2Pin1 = Pin(12, Pin.OUT)
motor2Pin2 = Pin(14, Pin.OUT)
# PIN for the Laser
laserPin= Pin(5, Pin.OUT)

# To make the LED flash
LED = Pin(2, Pin.OUT)   # onboard LED is PIN 2
 
    
########################################
# initialiseAll
########################################
def initialiseAll():

    print("initialiseAll")
    for x in range(4):
        LED.value(False)
        sleep(0.3)
        LED.value(True)


########################################
# runMotorsFromMsg
# message has 3 parts = left motor, right motor, laser (not used)
########################################
def runMotorsFromMsg(msg):
    
    if len(msg) > 2:
        setMotorPinsFromCommands(msg[0], msg[1], msg[2])
    else:
        setMotorPinsFromCommands(CMD_STOP, CMD_STOP, CMD_STOP)


########################################
# use the commands to set the pins that drive the motors
# left
########################################
def setMotorPinsFromCommands(left, right, laser):

    LED.value(False)
    if DEBUG:
        print("setMotorPinsFromCommands |"+str(left)+":"+str(right)+":"+str(laser)+"|")

    motor1_1 = False
    motor1_2 = False
    motor2_1 = False
    motor2_2 = False
    
    # Set the left motor
    if left == CMD_FWD:
        motor1_1 = True
    elif left == CMD_BACK:
        motor1_2 = True
    
    # Set the right motor
    if right == CMD_FWD:
        motor2_1 = True
    elif right == CMD_BACK:
        motor2_2 = True

    # Set the values of the motors
    setMotors(motor1_1, motor1_2, motor2_1, motor2_2)
        
    LED.value(True)
       
########################################
# set each of the 4 motors
########################################
def setMotors( motor1_1,  motor1_2,  motor2_1,  motor2_2):

    motor1Pin1.value(motor1_1)
    motor1Pin2.value(motor1_2)
    motor2Pin1.value(motor2_1)
    motor2Pin2.value(motor2_2)


########################################
# run a single motor test
########################################
def runMotorTest(test, msg, sleepTime):
    
    print(str(test))
    runMotorsFromMsg(msg)
    sleep (sleepTime)

########################################
# test if motors working
########################################
def testMotors():

    runMotorTest("Move forward", CMD_FWD + CMD_FWD + CMD_STOP, 2.0)
    runMotorTest("Move backward", CMD_BACK + CMD_BACK + CMD_STOP, 2.0)

    runMotorTest("Left Only Foward", CMD_FWD + CMD_STOP + CMD_STOP, 2.0)
    runMotorTest("Left Only backward",CMD_BACK + CMD_STOP + CMD_STOP, 2.0)

    runMotorTest("Right Only Foward", CMD_STOP + CMD_FWD + CMD_STOP, 2.0)
    runMotorTest("Right Only backward", CMD_STOP + CMD_BACK + CMD_STOP, 2.0)

    runMotorTest("Spin", CMD_FWD + CMD_BACK + CMD_STOP, 2.0)
    runMotorTest("Spin", CMD_BACK + CMD_FWD + CMD_STOP, 2.0)
 
    
    runMotorTest("STOP", CMD_STOP + CMD_STOP + CMD_STOP, 2.0)


##################################################
# start
##################################################

initialiseAll()

testMotors()