# test bluetooth Thisfile = "Drag1e2.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 185 # 8/7/22 log file writing enabled JF # also writes to bluetooth # also records time #### 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 ( could be bigger) 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 = 60 # Fall-off error (was 60) FOlevel = 15 # Fall-off sensor value # larger number makes it fall off sooner # fall-off error dictates curvature or steering back to line. ############# # 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 ############# # Bluetooth # ############# BT = UART(0, 9600) # BLUETooth init with given baudrate 0 = TX/RX pins tend=0 # end time tstart = 0 #start timr ############ # logtxt # ############ # record stuff to log file def logtext(): delay=time.ticks_diff(tend,tstart) 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)+" T="+str(loopcount/325)) pfile.write("\r\ndelay="+str(delay/1000)) pfile.close() print("\r\nlog.txt written") # output log file to Bluetooth BT.write("\r\n\r\nspeed="+str(speed)) BT.write("\r\nmult="+str(mult)) BT.write("\r\ndfactor="+str(dfactor)) BT.write("\r\nloops="+str(loopcount)+" T="+str(loopcount/325)+"\r\n") BT.write("\r\ndelay="+str(delay/1000)) BT.write("\r\nlog.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 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 loopcount=0 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 setspeeds(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 loopcount += 1 # count 1 more loop if ((loopcount%10)== 0): # every 10 loops (1 second) BT.write("\r\nHello") time.sleep(0.1) # wait 100 milliseconds BT.write("\r\nButton") ########## # race # ########## # function to run the Drag race def race(): global speed,mult,loopcount,tstart,tend # main loop to actually run the track loopcount=0 written=0 # signal that logfile not written print("Start") tstart = time.ticks_ms() 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 setspeeds(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 >= 1.3 seconds after start if (ssense > 40 and loopcount >400): # 1.3 seconds if(written == 0): tend = time.ticks_ms() # record finish time logtext() # write to log file and Bluetooth written=1 # record fact that file written 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