# file "test.py"
# 28/1/2022  JF
# tests all inputs and outputs
# Button, switches,
# inbuilt LED, LED2, LEDL, LEDR
# LineL, LineR, WallR

from machine import Pin, PWM, ADC
import time

##################### IO PORTS#####################
# buttons
Button = Pin(22, Pin.IN)                  #Hw29 UKmars function select back left
#switches
SW1 = Pin(2, Pin.IN, Pin.PULL_DOWN)       #Hw4 1st switch
SW2 = Pin(3, Pin.IN, Pin.PULL_DOWN)       #Hw5 1st switch
SW3 = Pin(4, Pin.IN, Pin.PULL_DOWN)       #Hw6 1st switch
SW4 = Pin(5, Pin.IN, Pin.PULL_DOWN)       #Hw7 1st switch


led  = Pin(25, Pin.OUT)   # inbuilt LED
led2 = Pin(20, Pin.OUT)   # side LED
ledR = Pin(6, Pin.OUT)    #  HW9 Sensor board  Right LED
ledL = Pin(11, Pin.OUT)   #  HW15 Sensor board  Left LED


# line sensors
rfront = ADC(Pin(26))     # HW31  right front sensor
lfront = ADC(Pin(27))     # HW32 left front sensor
#Wall sensors
LWALL = ADC(Pin(27))      # HW32 left front sensor
FWALL = ADC(Pin(26))      # HW31 front front sensor
RWALL = ADC(Pin(28))      # HW34 right front sensor
Trigger = Pin(12, Pin.OUT) #  HW16 Wall sensor LED trigger

### MAIN starts here ##################
# record current switch settings
OldButton = Button.value()
OldSW1  = SW1.value()
OldSW2  = SW2.value()
OldSW3  = SW3.value()
OldSW4  = SW4.value()

# set up Motor outputs
LPindir = Pin(7, Pin.OUT)    # forward pin object
LPinspeed = PWM(Pin(9))      # PWM pin object
LPinspeed.freq(1000)         # PWM frequency fixed at 1000
RPindir = Pin(8, Pin.OUT)    # forward pin object
RPinspeed = PWM(Pin(10))     # PWM pin object
RPinspeed.freq(1000)         # PWM frequency fixed at 1000


while True:
    if (OldButton != Button.value()):    # if Button has changed
        print ("Button=",Button.value()) # print new button state
        OldButton = Button.value()       # save current state for later
        if (OldButton == 1):
            led.on()                     # switch on in-built LED
            
            if (SW1.value() == 1) and( SW2.value() == 1):  # IF both SW1 and SW2 are on
                print("Trigger off:")    # show values without illumination
                print("LWALL",LWALL.read_u16(),"FWALL",FWALL.read_u16(),"RWALL",RWALL.read_u16(),)

                Trigger.on()             # show values with illumination
                print("Trigger on")
                time.sleep(0.1)          # wait for light to take effect
                print("LWALL",LWALL.read_u16(),"FWALL",FWALL.read_u16(),"RWALL",RWALL.read_u16(),)
                print("Trigger off.")
                Trigger.off()            # switch off when finished
				
            elif SW1.value() == 1:       # if only SW1
                print("led2 on")
                led2.on()

            elif SW2.value() == 1:       # if only SW2
                print("ledR,L on")
                ledR.on()                # switch on LEDs on sensor bar
                ledL.on()
                print("LWALL",LWALL.read_u16(),"FWALL",FWALL.read_u16(),"RWALL",RWALL.read_u16(),)
				
            elif SW4.value() == 1:
                print("Motors forward")
                LPindir.off()              # forwards
                LPinspeed.duty_u16(32000)  # half speed
                RPindir.off()              # forwards
                RPinspeed.duty_u16(32000)  # half speed
				
            elif SW3.value() == 1:
                print("Motors backward")
                LPindir.on()              # reverse
                LPinspeed.duty_u16(32000)  # half speed
                RPindir.on()              # reverse
                RPinspeed.duty_u16(32000)  # half speed

        else:                            # Button released:
            print("All LEDs off")        # switch off everything
            led2.off()
            led.off()
            ledR.off()
            ledL.off()
            print("Motors off")
            print(" ")
            LPindir.off()          # forwards
            LPinspeed.duty_u16(0)  # Zero speed
            
            RPindir.off()          # forwards
            RPinspeed.duty_u16(0)  # Zero speed

# This bit just shows changes in Switch settings
    if (OldSW1  != SW1.value()):
        OldSW1  = SW1.value()
        if (OldSW1 ==1):
            print ("SW1 on")
        else:
            print ("SW1 off")            

    if (OldSW2  != SW2.value()):
        OldSW2  = SW2.value()
        if (OldSW2 ==1):
            print ("SW2 on")
        else:
            print ("SW2 off")

    if (OldSW3  != SW3.value()):
        OldSW3  = SW3.value()
        if (OldSW3 ==1):
            print ("SW3 on")
        else:
            print ("SW3 off")

    if (OldSW4  != SW4.value()):
        OldSW4  = SW4.value()
        if (OldSW4 ==1):
            print ("SW4 on")
        else:
            print ("SW4 off")

#### End of test program