# test bluetooth Thisfile = "main_1July.py" # History # 25/4/2022 pre_race() and race() made into functions # 28/6/2022 # UKMARSb used for reads() # reads() called instead of calibs() in awaitstop and race # speed set to 0 to stop. line 155 # 1/7/22 extended Sensor regions setspeeds2() #### imports from machine import Pin, PWM, ADC,UART import time, utime from UKMARS import * # SW1, Button, lfront , rfront, Motor, calib[lrs](),reads() speed = 45 # early speed for 1/2 second latespeed = 100 # speed after start (also set by switches ) mult = 0.3 # used for tuning (0.3 is reasonable) dfactor = 43 # used for tuning (5 is reasonable) FOError = 80 # Fall-off error (was 60) FOlevel = 12 # Fall-off sensor value # larger number makes it fall off sooner # fall-off error dictates curvature or steering back to line. Const = 170 ############# # Data # ############# # motors LeftMotor = Motor("L") # set up left motor object RightMotor = Motor("R") # set up right motor object lspeed=0 # speed of left motor rspeed=0 # speed of right motor error = 0 # calculated from sensors OldError = 0 # used to track previous error change =0 loopcount = 0 ############ # logtxt # ############ # record stuff to log file def logtext(): pfile = open("log.txt","a") pfile.write("\r\n\r\nspeed="+str(speed)) pfile.write("\r\nmult="+str(mult)) pfile.write("\r\ndfactor="+str(dfactor)) pfile.write("\r\nloops="+str(loopcount)+" "+str(loopcount/160)) pfile.close() print("log.txt written") ################## # awaitstop # ################## # Function to await white paper under stop sensor # being inserted and removed def awaitstop(): ssense = reads(stopsense.read_u16()) # read stop sensor # await white paper while (ssense <60): ssense = reads(stopsense.read_u16()) # read stop sensor led.toggle() time.sleep(0.2) #await removal while (ssense >40): led.toggle() time.sleep(0.02) ssense = reads(stopsense.read_u16()) # read stop sensor led.off ################# # setspeeds() # ################# ''' Function to calculate error and motor speeds from the sensor values new for WKS9z detects fall-off at a tunable sensor level uses a tunable fall-off error value uses a tunable speed if fallen off ''' def setspeeds2(ls,rs): # ls and rs are the sensor readings global lspeed,rspeed # Python to let this function change speeds global error,OldError,mult # to allow changing error if (ls >FOlevel): # left >15 if (rs >FOlevel): # right >15 error = ls-rs # only if on-line, else: error = Const - ls led.off() myspeed = speed else: if (rs >FOlevel): # right >15 error = rs - Const led.off() myspeed = speed else: if error >0: error = Const - FOlevel else: error = FOlevel - Const led.on() myspeed = speed * 0.8 # run at fall-off speed error = error/2 val = error *mult # adjust steering amount # Derivative calculations, using last-time's value change = (error - OldError) # add in the derivative to val val = val + (change * dfactor)/10 lspeed = myspeed - val # speed of left motor rspeed = myspeed + val # speed of right motor OldError = error # keep track of this error for later return (error) # OLD VERSION ## def setspeeds(ls,rs): # ls and rs are the sensor readings global lspeed,rspeed # Python to let this function change speeds global error,OldError,mult # to allow changing error if (ls >FOlevel and rs >FOlevel): # detect not fallen off led.off() error = ls-rs # only if on-line, myspeed = speed else: led.on() # Fall-off detected # Off-line, so boost error if error >0: error = FOError else: error = -FOError myspeed = speed * 0.8 # run at fall-off speed val = error *mult # adjust steering amount # Derivative calculations, using last-time's value change = (error - OldError) # add in the derivative to val val = val + (change * dfactor)/10 lspeed = myspeed - val # speed of left motor rspeed = myspeed + val # speed of right motor OldError = error # keep track of this error for later return (error) ############## # pre_race # ############## def pre_race(): # pre race set-up loop to calibrate sensors, # print the workings, and check everything while Button.value()==0: # wait for Start button button to be pressed lsense = calibl(lfront.read_u16()) # read left front sensor rsense = calibr(rfront.read_u16()) # read right front sensor ssense = calibs(stopsense.read_u16()) # read stop sensor setspeeds2(lsense,rsense) # set up error from readings print ("L ",lsense," R ",rsense,"Error",error,"Stop",ssense) # print the sensor values and error # print ("Stop",ssense) # print the sensor values and error time.sleep(0.1) # wait 100 milliseconds ########## # race # ########## # function to run the Drag race def race(): global speed,mult,loopcount # main loop to actually run the track loopcount=0 stoploop= 10000 print("Start") while True: # Race LOOP - never stops if (loopcount == 80): # 1/2 second speed = latespeed mult = .2 lsense = calibl(lfront.read_u16()) # read left front sensor rsense = calibr(rfront.read_u16()) # read right front sensor ssense = reads(stopsense.read_u16()) # read stop sensor setspeeds2(lsense,rsense) # set up speeds from readings LeftMotor.speed(lspeed) # set left motor speed RightMotor.speed(rspeed) # set right motor speed loopcount = loopcount +1 time.sleep(0.002) # wait 5 milliseconds # stop on stop marker if >= 0.3 seconds after start if (ssense > 40 and loopcount >400): # 0.6 seconds speed=0 #RightMotor.stop() #stop the motors just in case #LeftMotor.stop() # stop motors ########## # main # ########## RightMotor.stop() #stop the motors just in case LeftMotor.stop() # stop motors print(Thisfile) pre_race() # pre-race loop, stops on push-button swval = readswitches() # read value of 4-way switch if swval != 150 : # ignore if ON,ON,ON,ON latespeed = swval*0.6 + 10 print("Latepeed="+str(latespeed)) print("Mult="+str(mult)) awaitstop() # wait for white paper under stop sensor race() # race ends 3 seconds after stop # end of program