# file "UKMARS.py"
# 14/4/2022  JF
# All the includes within 1 file
# all parameters in a seprate file params.py
# new function readswitches
# sensor RWALL added , LWALL, FWALL renamed
# sensor: stopsense is now ADC 2
# function ReadWALLS() added
# function CheckStop() added
# - Changes this version..
# calibs()
# 28/6/2020
#   reads() added
# .correction added

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

Trigger = Pin(12, Pin.OUT)   #  HW16 Wall sensor LED trigger

# sensors
rfront = ADC(Pin(26)) # HW31  right front sensor
lfront = ADC(Pin(27)) # HW32 left front sensor
stopsense = ADC(Pin(28)) # HW34 side stop sensor

LWALL = ADC(Pin(27)) # HW32 left front sensor
FWALL = ADC(Pin(26)) # HW31 front front sensor
RWALL = ADC(Pin(28)) # HW34 right front sensor

################
# readswitches #
################
## 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
    correction = 1.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.correction
        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 ###############
        

############
#  sensors #
############
minr= 65000
minl= 65000
mins= 65000
maxr = 0
maxl = 0
maxs = 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)

# stopsensor
def calibs(val):
    global mins,maxs
    outval = val
    if (val<mins)or(val>maxs):
        led.on()
        time.sleep(0.01)
    else:
        led.off()

    mins = min(val,mins)
    maxs = max(val,maxs)
    if (mins <maxs):
        outval = int((maxs-val)*100/(maxs-mins))
    if (outval  > 50):
        led2.on()
    else:
        led2.off()
    return min(outval,100)
    
# read stopsensor no calibration
def reads(val):
    global mins,maxs
    outval = val
    if (val<mins)or(val>maxs):
        led.on()
#        time.sleep(0.01)    # remove delay
    else:
        led.off()
        
# remove calibration:
#    mins = min(val,mins)
#    maxs = max(val,maxs)
    if (mins <maxs):
        outval = int((maxs-val)*100/(maxs-mins))
    if (outval  > 50):
        led2.on()
    else:
        led2.off()
    return min(outval,100)

###############
#  ReadWALLS  #
###############
def ReadWALLS():
    L= LWALL.read_u16()
    F= FWALL.read_u16()
    R= RWALL.read_u16()
    Trigger.on()
    time.sleep(0.005)           # wait 5ms for light to take effect
    L= LWALL.read_u16()-L
    F= FWALL.read_u16()-F
    R= RWALL.read_u16()-R
    Trigger.off()             # switch off illuminatoin
    return(L,F,R)             # return 3 values to calling program