# file "UKMARS.py"
# 14/12/2021 22:51 JF
# All the includes within 1 file
# all parameters in a seprate file params.py
# new function readswitches
# new sensor: stopsense
# calib r&l never generate more than 100


from machine import Pin, UART, PWM, ADC
import time

#uart = UART(0, 9600, timeout=1000)    # init with given baudrate 0 = TX/RX pins 100ms
uart = UART(0, 9600)    # init with given baudrate 0 = TX/RX pins 100ms

##################### 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)   #  HW14 Sensor board  Left LED

# sensors
rfront = ADC(Pin(26)) # right front sensor
lfront = ADC(Pin(27)) # left front sensor
stopsense = Pin(21, Pin.IN)   # HW27 QTR side stop sensor

## read switches as number####
def readswitches():
    swval = SW1.value() *10
    swval += SW2.value() *20
    swval += SW3.value() *40
    swval += SW4.value() *80
    return(swval)



#######################  Motors #####################
# Motor object to control 1 motor through UKMars motor controller
# updated 18/6/2021 John Fisher
# Use "L" or "R" to choose which motor

# provides pin numbers (fwd,rev,pwm)for each motor
#LeftMotor  = Motor("L")      # uses 7,9 to set up left motor object
#RightMotor = Motor("R")      # uses 8,10 to set up right motor object

class Motor:
# Handles pin numbers and fwd reverse
# Drives the Sparkfun motor driver board via the XOR invertion chip
    Pindir = 0
    Pinspeed = 0
    
    # constructor
    def __init__(self,type):   #tye="L" or "R"
        if ((type=="L")or(type=="l")):
            dir  = 7 # pin number for forwards motion
            spd  = 9 # pin number for speed control
        else:
            dir  = 8  # pin number for forwards motion
            spd  = 10 # pin number for speed control
            
        self.Pindir = Pin(dir, Pin.OUT)    # forward pin object
        self.Pinspeed = PWM(Pin(spd))      # PWM pin object
        self.Pinspeed.freq(1000)           # PWM frequency fixed at 1000
  
    # sets motor speed and direction range -100 to +100
    def speed(self,spd):
        if (spd >= 0):
            self.Pindir.off()   # sets pin low to go forward
        else:
            self.Pindir.on()  # set pin high to go backward
            spd =-spd
        spd = spd *655
        self.Pinspeed.duty_u16(int(min(spd,65535))) # set speed to speed
        
    def stop(self):   # provides braking
        self.Pindir.off()   # sets pin high to go forward
        self.Pinspeed.duty_u16(0)

############## Sample program ###############
        

############
#  sensor  #
############
# prints to bluetooth and USB
minr= 65000
minl= 65000
maxr = 0
maxl = 0

def calibl(val):
    global minl,maxl
    global ledL,ledR
    outval = val
    if (val<minl)or(val>maxl):
        led.on()
        time.sleep(0.01)
    else:
        led.off()
        
    minl = min(val,minl)
    maxl = max(val,maxl)
    if (minl <maxl):
        outval = int((maxl-val)*100/(maxl-minl))
    if (outval  > 20):
        ledL.on()
    else:
        ledL.off()
    return min(outval,100)

def calibr(val):
    global minr,maxr
    global ledL,ledR
    outval = val
    if (val<minr)or(val>maxr):
        led.on()
        time.sleep(0.01)
    else:
        led.off()

    minr = min(val,minr)
    maxr = max(val,maxr)
    if (minr <maxr):
        outval = int((maxr-val)*100/(maxr-minr))
    if (outval  > 20):
        ledR.on()
    else:
        ledR.off()
    return min(outval,100)