# file "UKMARS_New.py" # 28/9/2023 JF & T # Motor can have 2nd parameter = correction # uses version.py to tell it versions from machine import Pin, UART, PWM, ADC import time try: from version import * except: piggy="new" driver="new" class SW: # to reverse switch polarity PinObj = 0 # constructor def __init__(self,Num): #Num= GPIO of pin self.PinObj = Pin(Num, Pin.IN,Pin.PULL_UP) # pin object # value() reads inverted value: def value(self): return(1 - self.PinObj.value()) # set up with new (Nov/2022) piggyback board uart = UART(0, baudrate=9600, tx=Pin(16), rx=Pin(17), bits=8, parity=None, stop=1) # +++ if(piggy=="new"): print("New Pin numbers") uart = UART(0, baudrate=9600, tx=Pin(16), rx=Pin(17), bits=8, parity=None, stop=1) # +++ P_Button = 22 P_led = 25 P_led2 = 21 P_ledR = 8 P_ledL = 13 P_Trigger = 14 # ADC sensors are the Same #Switches SW_pull = Pin.PULL_UP P_SW1 = 0 P_SW2 = 1 P_SW3 = 2 P_SW4 = 3 SW1 = SW(P_SW1) #Hw1 1st switch +++ SW2 = SW(P_SW2) #Hw2 2nd switch +++ SW3 = SW(P_SW3) #Hw4 3rd switch +++ SW4 = SW(P_SW4) #Hw5 4th switch +++ #Motors P_LMdir=9 P_LMspd=11 P_RMdir=10 P_RMspd=12 lwheel = Pin(4,Pin.IN) rwheel = Pin(5,Pin.IN) else: # old pins print("Old Pin numbers") uart = UART(0, baudrate=9600) P_Button = 22 P_led = 25 P_led2 = 20 P_ledR = 6 P_ledL = 11 P_Trigger = 12 # ADC sensors are the Same #Switches SW_pull = Pin.PULL_DOWN P_SW1 = 2 P_SW2 = 3 P_SW3 = 4 P_SW4 = 5 SW1 = Pin(2, Pin.IN, Pin.PULL_DOWN) #Hw4 1st switch SW2 = Pin(3, Pin.IN, Pin.PULL_DOWN) #Hw5 2nd switch SW3 = Pin(4, Pin.IN, Pin.PULL_DOWN) #Hw6 3rd switch SW4 = Pin(5, Pin.IN, Pin.PULL_DOWN) #Hw7 4th switch #Motors P_LMdir=7 P_LMspd=9 P_RMdir=8 P_RMspd=10 pfile.close() lwheel = Pin(13,Pin.IN,Pin.PULL_UP) rwheel = Pin(14,Pin.IN,Pin.PULL_UP) if(driver=="new"): print("New Driver") else: print("Old Driver") ##################### IO PORTS##################### # buttons Button = Pin(P_Button, Pin.IN,Pin.PULL_DOWN) #Hw29 UKmars function select back left led = Pin(P_led, Pin.OUT) # inbuilt LED led2 = Pin(P_led2, Pin.OUT) # side LED +++ ledR = Pin(P_ledR, Pin.OUT) # Sensor board Right LED ledL = Pin(P_ledL, Pin.OUT) # Sensor board Left LED Trigger = Pin(P_Trigger, Pin.OUT) # HW16 Wall sensor LED trigger ''' 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) ############# # CheckStop # ############# stpThresh = 48000 markerCount = 0 overMarker = False def ClearStop(thresh=48000): global overMarker,markerCount,stpThresh stpThresh = thresh markerCount = 0 overMarker = False def CheckStop(): global overMarker,markerCount #print( stpThresh) if (stopsense.read_u16() < stpThresh): # if over marker if (overMarker == False): # if moving onto marker markerCount = markerCount+1 overMarker = True else: overMarker = False return(markerCount) ####################### 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 Rev = 0 Fwd = 0 correction = 1.0 Pindir = 0 Pinspeed = 0 # constructor def __init__(self,type,corr=1): #tye="L" or "R" self.correction = corr if ((type=="L")or(type=="l")): dir = P_LMdir # pin number for forwards motion spd = P_LMspd # pin number for speed control else: dir = P_RMdir # pin number for forwards motion spd = P_RMspd # pin number for speed control if driver=="new" : self.Rev = PWM(Pin(dir)) # forward PWM object self.Rev.freq(1000) # forward PWM object self.Fwd = PWM(Pin(spd)) # PWM pin object self.Fwd.freq(1000) # PWM frequency fixed at 1000 else: 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): spd = spd * self.correction if driver=="new": if (spd >= 0): self.Rev.duty_u16(0) # switch off reverse spd = spd *655 self.Fwd.duty_u16(int(min(spd,65535))) # set speed to speed else: self.Fwd.duty_u16(0) # switch off forwards spd =-spd spd = spd *655 self.Rev.duty_u16(int(min(spd,65535))) # set speed to speed else: 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 if driver=="new" : self.Rev.duty_u16(65535) # sets reverse pin high self.Fwd.duty_u16(65535) # sets forwards pin high else: 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 (valmaxl): led.on() time.sleep(0.01) else: led.off() minl = min(val,minl) maxl = max(val,maxl) if (minl 20): ledL.on() else: ledL.off() return min(outval,100) def calibr(val): global minr,maxr global ledL,ledR outval = val if (valmaxr): led.on() time.sleep(0.01) else: led.off() minr = min(val,minr) maxr = max(val,maxr) if (minr 20): ledR.on() else: ledR.off() return min(outval,100) # stopsensor def calibs(val): global mins,maxs outval = val if (valmaxs): led.on() time.sleep(0.01) else: led.off() mins = min(val,mins) maxs = max(val,maxs) if (mins 50): led2.on() else: led2.off() return min(outval,100) # read stopsensor no calibration def reads(val): global mins,maxs outval = val if (valmaxs): led.on() # time.sleep(0.01) # remove delay else: led.off() # remove calibration: # mins = min(val,mins) # maxs = max(val,maxs) if (mins 50): led2.on() else: led2.off() return min(outval,100) old_l = 0 old_r = 0 old_f = 0 ############### # ReadWALLS # ############### def ReadWALLS(): global old_l, old_r, old_f 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 time.sleep(0.005) #L, old_l, R, old_r, F, old_f = old_l, L, old_r, R, old_f, F return(L, F, R) #return((L + old_l) / 2, (F + old_f) / 2, (R + old_r) / 2) # return 3 values to calling program