# test bluetooth Thisfile = "DRAG6.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 #19/7/222 interrupt used to detect 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 # pins stp = Pin(28, Pin.IN) # stop sensor digital 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) looptime 3ms dfactor = 8 # used for tuning (5 is reasonable) looptime 10ms 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 Markers = 0 ssense = 0 ############# # Bluetooth # ############# BT = UART(0, 9600) # BLUETooth init with given baudrate 0 = TX/RX pins tend=0 # end time of marker tstart = 0 #start timr of marker ################ # interrupt fn # ################ # runs on stp pin going high or low def stpirq(fred): global tstart,tend,Markers,ssense if (stp.value() == 1): # Black led2.off() ssense = 0 tend = time.ticks_ms() else: # white led2.on() ssense = 100 Markers+=1 tstart = time.ticks_ms() ############ # 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 # 19/7/2022 Digital version def awaitstop(): # await white paper while (stp.value() == 1): # Black led.toggle() time.sleep(0.1) #await removal while (stp.value() == 0): # White led.toggle() time.sleep(0.1) 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,Markers,StartCount,latespeed # main loop to actually run the track loopcount=0 written=0 # signal that logfile not written print("Start") tstart = time.ticks_ms() StartCount = Markers # count of Markers at start 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 - StartCount) >=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) # set up interrupts stp.irq(handler=stpirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING) 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 ## second thread #_thread.start_new_thread(Thread2, ()) race() # race ends when speed = 0 # end of program