#30/03/2023 ######### # Setup # ######### #imports from UKMARS import * from tvr_lib import Remote import solver_m # Setup for maze solving lwheel = Pin(13,Pin.IN,Pin.PULL_UP) rwheel = Pin(14,Pin.IN,Pin.PULL_UP) lmotor = Motor("L") rmotor = Motor("R") rmotor.correction = 0.95 LDistance=0 RDistance=0 step = 0 command = 0 speed = 20 present = 4000 lpresent = 3700 rpresent = 4000 #Steup for wall follwing RightMotor = rmotor LeftMotor = lmotor mult = 0.001 # amount of steering in corridor delay = 0.18 #time delay for left_delay frontvalue = 14000 #front sensor value for breaking corridor leftvalue = 3400 #left sensor value for breaking corridor ############################ # Wall Following Functions # ############################ # corridor() def corridor(): # go down the corridor global left, front, right while True: # this version doesn't stop (left,front,right)= ReadWALLS() # read all 3 walls target = 6000 # required left wall distance # statements to end the loop (not used till step4) if (left < leftvalue): RightMotor.stop() LeftMotor.stop() break # exit loop if no left wall if (front > frontvalue): RightMotor.stop() LeftMotor.stop() break # exit loop if front wall error = left - target # subtract target error = error * mult # amount of steering lspeed = speed + error # calculate motor speeds rspeed = speed - error RightMotor.speed(rspeed) # send speeds to motors LeftMotor.speed(lspeed) time.sleep(0.01)# do a hundred readings per second print("exit", front, left) # left_delay() def left_delay(): global left #delay before lspeed = 32 rspeed = 30 RightMotor.speed(rspeed) LeftMotor.speed(lspeed) time.sleep(delay) # adjsut to suit #turn lspeed = 15 # adjust to suit rspeed = 40 # adjust to suit RightMotor.speed(rspeed) LeftMotor.speed(lspeed) (left,front,right)= ReadWALLS() # read all 3 walls while left < leftvalue: (left,front,right)= ReadWALLS() # read all 3 walls time.sleep(0.01) RightMotor.stop() LeftMotor.stop() # right_delay() def right_delay(): global front lspeed = 30 # adjust to suit rspeed = -30 # adjust to suit RightMotor.speed(rspeed) LeftMotor.speed(lspeed) (left,front,right)= ReadWALLS() # read all 3 walls while front > 7000: (left,front,right)= ReadWALLS() # read all 3 walls time.sleep(0.01) # adjsut to suit led.on() led.off() RightMotor.stop() LeftMotor.stop() ########################## # Maze Solving Functions # ########################## # Setup for left wheel sensor def Lwheelirq(which): global LDistance # count of wheel markers ledL.value(lwheel.value()) # light the sensor bar LED accordingly LDistance += 1 # increase count # Setup for left wheel sensor def Rwheelirq(which): global RDistance # count of wheel markers ledR.value(rwheel.value()) # light the sensor bar LED accordingly RDistance += 1 # increase count # Whats Next def remote(): global remote while True: char = remote.get() if char: return char # forward() def forward(dist): global LDistance, RDistance LDistance=0 RDistance=0 lmotor.speed(30) rmotor.speed(30) while LDistance < dist or RDistance < dist: (left,front,right)= ReadWALLS() time.sleep(0.01) #Stearing if left > lpresent: L = 1 else: L = 0 if right > rpresent: R = 1 else: R = 0 if L == 1: error = left - ltarget # subtract target error = error * mult # amount of steering lspeed = speed + error # calculate motor speeds rspeed = speed - error elif R == 1: error = right - rtarget # subtract target error = error * mult # amount of steering lspeed = speed - error # calculate motor speeds rspeed = speed + error else: lspeed = speed rspeed = speed RightMotor.speed(rspeed) # send speeds to motors LeftMotor.speed(lspeed) ''' if there is left wall stear to left target if there is right wall stear to right target no walls? go straught ahead ''' lmotor.speed(0) rmotor.speed(0) # left_turn() def left_turn(): global LDistance, RDistance LDistance=0 RDistance=0 rmotor.speed(30) lmotor.speed(0) while RDistance < 16: pass forward(16) lmotor.speed(0) rmotor.speed(0) # right_turn() def right_turn(): global LDistance, RDistance LDistance=0 RDistance=0 lmotor.speed(30) rmotor.speed(0) while LDistance < 18: pass forward(16) lmotor.speed(0) rmotor.speed(0) # u_turn() def u_turn(): global LDistance, RDistance LDistance=0 RDistance=0 rmotor.speed(-30) lmotor.speed(30.5) while RDistance < 18: pass forward(13) lmotor.speed(0) rmotor.speed(0) ######## # Main # ######## # set up interrupt handler for each pin lwheel.irq(handler=Lwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING) rwheel.irq(handler=Rwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING) #bluetooth or TV remote if SW2.value() == 1: remote = Remote(0,9600) # Bluetooth else: remote = Remote(0) # TV Remote #how to start the robot if SW4.value() == 0: command = remote.get() while command != 'P': (left,front,right)= ReadWALLS() time.sleep(0.1) print("T L:",left,"F:",front,"R:",right) command = remote.get() print(command) else: while Button.value() == 0: (left,front,right)= ReadWALLS() time.sleep(0.1) print("B L:",left,"F:",front,"R:",right) #print(command) time.sleep(0.5) (left,front,right)= ReadWALLS() #used for steering ltarget = left ftarget = front rtarget = right f = open("log.txt", "w") f.write("Starting\n\n") f.close() while True: if SW1.value() == 1: # wall follower corridor() (left,front,right)= ReadWALLS() if left < leftvalue: left_delay() else: right_delay() else: (left,front,right)= ReadWALLS() if left > present: L = 1 else: L = 0 if front > present: F = 1 else: F = 0 if right > present: R = 1 else: R = 0 if SW3.value() == 1: command = remote.get() while command == '?': command = remote.get() else: command = solver_m.wn(L,F,R) print(command) f = open("log.txt", "a") f.write(str(L)+","+str(F)+","+str(R)+"\n") f.write(str(command)+"\n") f.close() if command == '+': forward(28) #time.sleep(0.5) elif command == '>': right_turn() #time.sleep(0.5) elif command == '<': left_turn() #time.sleep(0.5) elif command == 'O': u_turn() #time.sleep(0.5)