# test bluetooth Thisfile = "DRAG5.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 #16/7/22 2nd thread used to count stop markers #### imports from machine import Pin, PWM, ADC,UART import time, utime from UKMARS import * # SW1, Button, lfront , rfront, Motor, calib[lrs](),reads() import _thread # used to start the second thread import gc # garbage collection speed = 95 # 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) dfactor = 8 # 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 # Global data shared with Thread2: ClearCount = True # tells thread2 to clear the count Markers = 0 # set by Th2. counts markers MarkerMs = 0 # length of last marker ############# # Bluetooth # ############# BT = UART(0, 9600) # BLUETooth init with given baudrate 0 = TX/RX pins tend=0 # end time tstart = 0 #start timr ################# # Second Thread # ################# # Interface: # Markers written by Th2 read TH1 count of number of markers # ClearCount (flag) written by TH1 read and reset by TH2 # MarkerMs written by TH2 read by TH1 time duration of most recent marker # stopsense ADC sensor to be read by TH2 def Thread2(): global Markers,ClearCount,MarkerMs print("Th2 starting") while True: ssense = reads(stopsense.read_u16()) # read stop sensor while (ssense < 60): ssense = reads(stopsense.read_u16()) # read stop sensor time.sleep(.001) if (ClearCount == True ): # if flag set ClearCount = False # clear flag Markers = 0 # reset count # marker started...... increment count Markers += 1 tstart = time.ticks_ms() # record start time while (ssense >40): ssense = reads(stopsense.read_u16()) # read stop sensor time.sleep(.001) if (ClearCount == True ): ClearCount = False Markers = 0 MarkerMs=time.ticks_diff(time.ticks_ms(),tstart) # record length ## marker ended time.sleep(.001) ############ # 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/330)+"\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 loops=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 time.sleep(0.1) # wait 100 milliseconds loops+=1 if (loops%10 == 0): print ("hello") BT.write("hello "+str(int(loops/10)) +"\r\n") ########## # race # ########## # function to run the Drag race def race(): global speed,mult,loopcount,tstart,tend,ClearCount,Markers,latespeed # main loop to actually run the track loopcount=0 written=0 # signal that logfile not written print("Start") tstart = time.ticks_ms() ClearCount = True # tells thread2 to clear the count while True: # Race LOOP - never stops gc.collect( ) #print("GH") 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.01) # wait 12 milliseconds if (loopcount == 20): # 1/4 second speed = latespeed mult = .2 # stop on stop marker if >= 1.3 seconds after start if (Markers >=2): # if passed start and stop markers if(written == 0): tend = time.ticks_ms() # record finish time LeftMotor.speed(-20) # set left motor speed RightMotor.speed(-20) # set right motor speed logtext() # write to log file and Bluetooth written=1 # record fact that file written speed=-30 # LoopFinal = loopcount + 150 # half second LoopFinal = loopcount + 25 # quarter second ClearCount = True # tells thread2 to clear the count print(loopcount,LoopFinal) # final stop at the very end if ((speed <0) and loopcount >= LoopFinal): speed = 0 latespeed = 0 print("Stopped") if (loopcount%20 == 0): print(speed) ########## # 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 Markers = 0 _thread.start_new_thread(Thread2, ()) race() # race ends when speed = 0 # end of program