def GetEncCounts(): EncArray = [] EncArray.append(int(BFR4WDserialport.sendcommand("G1"))) EncArray.append(int(BFR4WDserialport.sendcommand("G2"))) EncArray.append(int(BFR4WDserialport.sendcommand("G3"))) EncArray.append(int(BFR4WDserialport.sendcommand("G4"))) return EncArray
def updateIR(self): LeftIR = BFR4WDserialport.sendcommand('G6') RightIR = BFR4WDserialport.sendcommand('G7') self.irCanvas.delete("all") YLeft = (int(LeftIR)/4) YRight = (int(RightIR)/4) self.irCanvas.create_rectangle(36,152,56,YLeft, fill='red') self.irCanvas.create_rectangle(96,152,116,YRight, fill='red') self.returnEntry.delete(0,END) self.returnEntry.insert(10, 'Left = ' + str(LeftIR) + ' Right = ' + str(RightIR)) self.after(self.updateInterval, self.updateIR)
def SonarSweep(startAngle, endAngle, stepSize, tilt, filename): Heading = BFR4WDserialport.sendcommand("G8") #get robot heading for x in range(startAngle, endAngle, stepSize): string = "H1 P"+ str(x) + "T"+ str(tilt) #Form string for head move command returned = BFR4WDserialport.sendcommand(string) #move head sonar = int(BFR4WDserialport.sendcommand("G5")) #get sonar reading thetaP = int(Heading) + x #calculate bearing of sonar reading thetaT = tilt data = [xrobot, yrobot, thetaP, thetaT, sonar] #form data list print data writetofile(filename,data) #write data list to file
def updateIR(self): LeftIR = BFR4WDserialport.sendcommand('G6') RightIR = BFR4WDserialport.sendcommand('G7') self.irCanvas.delete("all") YLeft = (int(LeftIR) / 4) YRight = (int(RightIR) / 4) self.irCanvas.create_rectangle(36, 152, 56, YLeft, fill='red') self.irCanvas.create_rectangle(96, 152, 116, YRight, fill='red') self.returnEntry.delete(0, END) self.returnEntry.insert( 10, 'Left = ' + str(LeftIR) + ' Right = ' + str(RightIR)) self.after(self.updateInterval, self.updateIR)
def SonarSweep(startAngle, endAngle, stepSize, tilt, filename): Heading = BFR4WDserialport.sendcommand("G8") #get robot heading for x in range(startAngle, endAngle, stepSize): string = "H1 P" + str(x) + "T" + str( tilt) #Form string for head move command returned = BFR4WDserialport.sendcommand(string) #move head sonar = int(BFR4WDserialport.sendcommand("G5")) #get sonar reading thetaP = int(Heading) + x #calculate bearing of sonar reading thetaT = tilt data = [xrobot, yrobot, thetaP, thetaT, sonar] #form data list print data writetofile(filename, data) #write data list to file
def SonarScan(): SonarArray = [] returned = BFR4WDserialport.sendcommand("S1 V1") #Servo power on returned = BFR4WDserialport.sendcommand("S2 V10") for x in range(-80,80,5): string = "H1 P"+ str(x) + " T0" returned = BFR4WDserialport.sendcommand(string) sonar = int(BFR4WDserialport.sendcommand("G5")) SonarArray.append(sonar) returned = BFR4WDserialport.sendcommand("S1 V0") #Servo power off return SonarArray
def SonarScan(): SonarArray = [] returned = BFR4WDserialport.sendcommand("S1 V1") #Servo power on returned = BFR4WDserialport.sendcommand("S2 V10") for x in range(-80, 80, 5): string = "H1 P" + str(x) + " T0" returned = BFR4WDserialport.sendcommand(string) sonar = int(BFR4WDserialport.sendcommand("G5")) SonarArray.append(sonar) returned = BFR4WDserialport.sendcommand("S1 V0") #Servo power off return SonarArray
def RunSequence(seq): imagecounter = 0 for x in seq: print x if 'IMGSAVE' in x: time.sleep(0.2) #Let image settle filename = 'Images/'+str(imagecounter)+".png" print "File Name: ", filename BFR4WDOpenCV.SaveFrame(filename) imagecounter += 1 else: returned = BFR4WDserialport.sendcommand(x) if 'E0' in returned: print "Command Complete" elif 'E1' in returned: print "Invalid command" return elif 'E2' in returned: print "Obstacle: Sonar" return elif 'E3' in returned: print "Obstacle: Left IR" return elif 'E4' in returned: print "Obstacle: Right IR" return
def RunSequence(seq): imagecounter = 0 for x in seq: print x if 'IMGSAVE' in x: time.sleep(0.2) #Let image settle filename = 'Images/' + str(imagecounter) + ".png" print "File Name: ", filename BFR4WDOpenCV.SaveFrame(filename) imagecounter += 1 else: returned = BFR4WDserialport.sendcommand(x) if 'E0' in returned: print "Command Complete" elif 'E1' in returned: print "Invalid command" return elif 'E2' in returned: print "Obstacle: Sonar" return elif 'E3' in returned: print "Obstacle: Left IR" return elif 'E4' in returned: print "Obstacle: Right IR" return
def updateCompass(self): self.compassControl.create_oval(25, 25, 127, 127, fill="white",outline="white") returned = BFR4WDserialport.sendcommand('G8') anglerad = math.radians(float(returned)) circlex = 76 + (math.sin(anglerad)*51) circley = 76 - (math.cos(anglerad)*51) self.compassControl.create_line(76, 76, circlex, circley, fill="red", width=3) if self.oktoupdatecompass: self.after(self.updateInterval, self.updateCompass)
def updateSonar(self): returned = BFR4WDserialport.sendcommand('G5') self.sonarCanvas.delete("all") y = 152-(int(returned)/1.5) if int(returned) <= 40: colour = 'red' elif int(returned) > 40 and int(returned) <= 80: colour = 'orange' else: colour = 'green' self.sonarCanvas.create_polygon((76,152),(56,y),(96,y), fill=colour) self.sonarCanvas.create_text(20,132,fill = colour,text = str(returned) + 'cm') self.after(self.updateInterval, self.updateSonar)
def manualmode(): while True: command = raw_input("Command: ") if 'exit' in command: break returned = BFR4WDserialport.sendcommand(command) if 'E0' in returned: print "Command Complete" elif 'E1' in returned: print "Invalid Command" else: print returned
def updateSonar(self): returned = BFR4WDserialport.sendcommand('G5') self.sonarCanvas.delete("all") y = 152 - (int(returned) / 1.5) if int(returned) <= 40: colour = 'red' elif int(returned) > 40 and int(returned) <= 80: colour = 'orange' else: colour = 'green' self.sonarCanvas.create_polygon((76, 152), (56, y), (96, y), fill=colour) self.sonarCanvas.create_text(20, 132, fill=colour, text=str(returned) + 'cm') self.after(self.updateInterval, self.updateSonar)
def updateCompass(self): self.compassControl.create_oval(25, 25, 127, 127, fill="white", outline="white") returned = BFR4WDserialport.sendcommand('G8') anglerad = math.radians(float(returned)) circlex = 76 + (math.sin(anglerad) * 51) circley = 76 - (math.cos(anglerad) * 51) self.compassControl.create_line(76, 76, circlex, circley, fill="red", width=3) if self.oktoupdatecompass: self.after(self.updateInterval, self.updateCompass)
def RunFileSequence(filename): f = open(filename, 'r') imagecounter = 0 for x in f: command = x.split("#")[0].rstrip( '\n') #remove comments and newline characters if not command: #do nothing if remaining string is empty pass else: print command if 'IMGSAVE' in x: time.sleep(0.3) #Let image settle filename = 'Images/' + str(imagecounter) + ".png" print "File Name: ", filename BFR4WDOpenCVGui.SaveFrame(filename) imagecounter += 1 else: returned = BFR4WDserialport.sendcommand(command) if 'E0' in returned: print "Command Complete" elif 'E1' in returned: print "Invalid command" return elif 'E2' in returned: print "Obstacle: Sonar" return elif 'E3' in returned: print "Obstacle: Left IR" return elif 'E4' in returned: print "Obstacle: Right IR" return
def SonarSweep(startAngle, endAngle, stepSize, tilt): dataarray = [] returned = BFR4WDserialport.sendcommand("S1V1") # servo power on returned = BFR4WDserialport.sendcommand("S3V10") # Head speed for x in range(startAngle, endAngle, stepSize): Heading = BFR4WDserialport.sendcommand("G8") # get robot heading string = "H1 P" + str(x) + "T" + str(tilt) # Form string for head move command returned = BFR4WDserialport.sendcommand(string) # move head sonar = int(BFR4WDserialport.sendcommand("G5")) # get sonar reading thetaP = int(Heading) + x # calculate bearing of sonar reading using compass data and servo angle thetaT = tilt data = [thetaP, thetaT, sonar] # form data list dataarray.append(data) returned = BFR4WDserialport.sendcommand("S1V0") # servo power off return dataarray
def RunFileSequence(filename): f = open(filename, 'r') imagecounter = 0 for x in f: command = x.split("#")[0].rstrip('\n') #remove comments and newline characters if not command: #do nothing if remaining string is empty pass else: print command if 'IMGSAVE' in x: time.sleep(0.3) #Let image settle filename = 'Images/'+str(imagecounter)+".png" print "File Name: ", filename BFR4WDOpenCV.SaveFrame(filename) imagecounter += 1 else: returned = BFR4WDserialport.sendcommand(command) if 'E0' in returned: print "Command Complete" elif 'E1' in returned: print "Invalid command" return elif 'E2' in returned: print "Obstacle: Sonar" return elif 'E3' in returned: print "Obstacle: Left IR" return elif 'E4' in returned: print "Obstacle: Right IR" return
def SonarSweep(startAngle, endAngle, stepSize, tilt): dataarray = [] returned = BFR4WDserialport.sendcommand('S1V1') #servo power on returned = BFR4WDserialport.sendcommand('S3V10') #Head speed for x in range(startAngle, endAngle, stepSize): Heading = BFR4WDserialport.sendcommand("G8") #get robot heading string = "H1 P" + str(x) + "T" + str( tilt) #Form string for head move command returned = BFR4WDserialport.sendcommand(string) #move head sonar = int(BFR4WDserialport.sendcommand("G5")) #get sonar reading thetaP = int( Heading ) + x #calculate bearing of sonar reading using compass data and servo angle thetaT = tilt data = [thetaP, thetaT, sonar] #form data list dataarray.append(data) returned = BFR4WDserialport.sendcommand('S1V0') #servo power off return dataarray
def sendCommand(self, event): returned = BFR4WDserialport.sendcommand(self.commandEntry.get()) self.returnEntry.delete(0, END) self.returnEntry.insert(10, returned) self.commandEntry.delete(0, END)
def GetIR(): dataarray = [] Heading = BFR4WDserialport.sendcommand("G8") # get robot heading Left = int(BFR4WDserialport.sendcommand("G6")) # get left IR reading
def GetIR(): dataarray = [] Heading = BFR4WDserialport.sendcommand("G8") #get robot heading Left = int(BFR4WDserialport.sendcommand("G6")) #get left IR reading
thetaP = int(Heading) + x #calculate bearing of sonar reading thetaT = tilt data = [xrobot, yrobot, thetaP, thetaT, sonar] #form data list print data writetofile(filename, data) #write data list to file ######################################################################################## # # Main # ######################################################################################## f = open('RawMapData.txt', 'w') #Open a file Initialise(10, 10) Heading = BFR4WDserialport.sendcommand("G8") #get initial robot heading SonarArray = SonarSweep(-60, 60, 10, 0, f) returned = BFR4WDserialport.sendcommand("W1 D50") #Forward 50cm FLdistance = BFR4WDserialport.sendcommand("G1") RLdistance = BFR4WDserialport.sendcommand("G2") FRdistance = BFR4WDserialport.sendcommand("G3") RRdistance = BFR4WDserialport.sendcommand("G4") averageddistance = (int(FLdistance) + int(RLdistance) + int(FRdistance) + int(RRdistance)) / 4 Headingrads = math.radians(float(Heading)) xrobot = xrobot + int(math.sin(Headingrads) * averageddistance) yrobot = yrobot + int( math.cos(Headingrads) * averageddistance) #calculate new robot x and y coordinates
def GetIR(): IRArray = [] IRArray.append(int(BFR4WDserialport.sendcommand("G6"))) IRArray.append(int(BFR4WDserialport.sendcommand("G7"))) return IRArray
def commandHeading(self, event): returned = BFR4WDserialport.sendcommand('W7V' + str(self.compassAngle)) self.returnEntry.insert(10, returned) self.oktoupdatecompass = True self.after(self.updateInterval, self.updateCompass ) #restart drawing actual value after new angle is sent
def moveClick(self,event): self.returnEntry.delete(0,END) # Speed select bar clicked if event.y > 10 and event.y < 40: if event.x >= 5 and event.x < 30: returned = BFR4WDserialport.sendcommand('S2V2') self.returnEntry.insert(10,returned) elif event.x >= 30 and event.x < 55: returned = BFR4WDserialport.sendcommand('S2V4') self.returnEntry.insert(10,returned) elif event.x >= 55 and event.x < 79: returned = BFR4WDserialport.sendcommand('S2V6') self.returnEntry.insert(10,returned) elif event.x >= 79 and event.x < 104: returned = BFR4WDserialport.sendcommand('S2V8') self.returnEntry.insert(10,returned) elif event.x >= 104 and event.x < 126: returned = BFR4WDserialport.sendcommand('S2V10') self.returnEntry.insert(10,returned) #Forward bar clicked if event.x > 133 and event.x < 170: if event.y >= 5 and event.y < 38: returned = BFR4WDserialport.sendcommand('W1D50') self.returnEntry.insert(10,returned) if event.y >= 38 and event.y < 70: returned = BFR4WDserialport.sendcommand('W1D30') self.returnEntry.insert(10,returned) if event.y >= 70 and event.y < 102: returned = BFR4WDserialport.sendcommand('W1D10') self.returnEntry.insert(10,returned) if event.y >= 102 and event.y < 133: returned = BFR4WDserialport.sendcommand('W1D5') self.returnEntry.insert(10,returned) #Reverse bar clicked if event.y >= 170 and event.y < 204: returned = BFR4WDserialport.sendcommand('W2D5') self.returnEntry.insert(10,returned) if event.y >= 204 and event.y < 235: returned = BFR4WDserialport.sendcommand('W2D10') self.returnEntry.insert(10,returned) if event.y >= 235 and event.y < 268: returned = BFR4WDserialport.sendcommand('W2D30') self.returnEntry.insert(10,returned) if event.y >= 268 and event.y < 300: returned = BFR4WDserialport.sendcommand('W2D50') self.returnEntry.insert(10,returned) #Turn ACW bar clicked if event.y > 134 and event.y < 172: if event.x >= 5 and event.x < 38: returned = BFR4WDserialport.sendcommand('W3D120') self.returnEntry.insert(10,returned) if event.x >= 38 and event.x < 70: returned = BFR4WDserialport.sendcommand('W3D90') self.returnEntry.insert(10,returned) if event.x >= 70 and event.x < 102: returned = BFR4WDserialport.sendcommand('W3D45') self.returnEntry.insert(10,returned) if event.x >= 102 and event.x < 133: returned = BFR4WDserialport.sendcommand('W3D30') self.returnEntry.insert(10,returned) #Turn CW bar clicked if event.x >= 170 and event.x < 204: returned = BFR4WDserialport.sendcommand('W4D30') self.returnEntry.insert(10,returned) if event.x >= 204 and event.x < 235: returned = BFR4WDserialport.sendcommand('W4D45') self.returnEntry.insert(10,returned) if event.x >= 235 and event.x < 268: returned = BFR4WDserialport.sendcommand('W4D90') self.returnEntry.insert(10,returned) if event.x >= 268 and event.x < 300: returned = BFR4WDserialport.sendcommand('W4D120') self.returnEntry.insert(10,returned)
mode = raw_input("Mode: ") if mode == 'M': manualmode() elif mode == 'A': time.sleep(3) RunSequence(["S1V1","S2 V8","S3 V4","H1 P0 T0", "W1 D300", "H1 P-80", "W1 D300", "W2 D300","S1 V25","H1 P0","W1 D400","S1V0"]) elif mode == 'B': time.sleep(3) RunSequence(["S1V1","S2 V15","S3 V3", "W1 D200","W3 D160","W1 D200","W3 D160","W1 D200","W3 D160","W1 D200","W3 D160","S1V0"]) elif mode == 'S': SonarArray = SonarScan() print SonarArray elif mode == 'T': RunFileSequence('BFRCode/Testfile') elif mode == 'I': BFR4WDOpenCV.DisplayFrame() BFR4WDserialport.closeserial()
def headClick(self,event): self.returnEntry.delete(0,END) # Speed select bar clicked if event.y > 10 and event.y < 40: if event.x >= 5 and event.x < 30: returned = BFR4WDserialport.sendcommand('S3V2') self.returnEntry.insert(10,returned) elif event.x >= 30 and event.x < 55: returned = BFR4WDserialport.sendcommand('S3V4') self.returnEntry.insert(10,returned) elif event.x >= 55 and event.x < 79: returned = BFR4WDserialport.sendcommand('S3V6') self.returnEntry.insert(10,returned) elif event.x >= 79 and event.x < 104: returned = BFR4WDserialport.sendcommand('S3V8') self.returnEntry.insert(10,returned) elif event.x >= 104 and event.x < 126: returned = BFR4WDserialport.sendcommand('S3V10') self.returnEntry.insert(10,returned) #Head tilt up clicked if event.x > 133 and event.x < 170: if event.y >= 5 and event.y < 38: returned = BFR4WDserialport.sendcommand('H1T75') self.returnEntry.insert(10,returned) if event.y >= 38 and event.y < 70: returned = BFR4WDserialport.sendcommand('H1T50') self.returnEntry.insert(10,returned) if event.y >= 70 and event.y < 102: returned = BFR4WDserialport.sendcommand('H1T25') self.returnEntry.insert(10,returned) if event.y >= 102 and event.y < 133: returned = BFR4WDserialport.sendcommand('H1T10') self.returnEntry.insert(10,returned) #Centralise head pan and tilt if central section is clicked if event.y >= 133 and event.y < 170: returned = BFR4WDserialport.sendcommand('H1T0P0') self.returnEntry.insert(10,returned) #Head tilt down clicked if event.y >= 170 and event.y < 204: returned = BFR4WDserialport.sendcommand('H1T-10') self.returnEntry.insert(10,returned) if event.y >= 204 and event.y < 235: returned = BFR4WDserialport.sendcommand('H1T-25') self.returnEntry.insert(10,returned) if event.y >= 235 and event.y < 268: returned = BFR4WDserialport.sendcommand('H1T-50') self.returnEntry.insert(10,returned) if event.y >= 268 and event.y < 300: returned = BFR4WDserialport.sendcommand('H1T-75') self.returnEntry.insert(10,returned) #Head pan ACW clicked if event.y > 134 and event.y < 172: if event.x >= 5 and event.x < 38: returned = BFR4WDserialport.sendcommand('H1P-75') self.returnEntry.insert(10,returned) if event.x >= 38 and event.x < 70: returned = BFR4WDserialport.sendcommand('H1P-50') self.returnEntry.insert(10,returned) if event.x >= 70 and event.x < 102: returned = BFR4WDserialport.sendcommand('H1P-25') self.returnEntry.insert(10,returned) if event.x >= 102 and event.x < 133: returned = BFR4WDserialport.sendcommand('H1P-10') self.returnEntry.insert(10,returned) #Head pan CW clicked if event.x >= 170 and event.x < 204: returned = BFR4WDserialport.sendcommand('H1P10') self.returnEntry.insert(10,returned) if event.x >= 204 and event.x < 235: returned = BFR4WDserialport.sendcommand('H1P25') self.returnEntry.insert(10,returned) if event.x >= 235 and event.x < 268: returned = BFR4WDserialport.sendcommand('H1P50') self.returnEntry.insert(10,returned) if event.x >= 268 and event.x < 300: returned = BFR4WDserialport.sendcommand('H1P75') self.returnEntry.insert(10,returned)
def commandHeading(self,event): returned = BFR4WDserialport.sendcommand('W7V' + str(self.compassAngle)) self.returnEntry.insert(10,returned) self.oktoupdatecompass = True self.after(self.updateInterval, self.updateCompass) #restart drawing actual value after new angle is sent
def servoOff(self,event): returned = BFR4WDserialport.sendcommand('S1V0') self.returnEntry.delete(0,END) self.returnEntry.insert(10,returned)
def moveClick(self, event): self.returnEntry.delete(0, END) # Speed select bar clicked if event.y > 10 and event.y < 40: if event.x >= 5 and event.x < 30: returned = BFR4WDserialport.sendcommand('S2V2') self.returnEntry.insert(10, returned) elif event.x >= 30 and event.x < 55: returned = BFR4WDserialport.sendcommand('S2V4') self.returnEntry.insert(10, returned) elif event.x >= 55 and event.x < 79: returned = BFR4WDserialport.sendcommand('S2V6') self.returnEntry.insert(10, returned) elif event.x >= 79 and event.x < 104: returned = BFR4WDserialport.sendcommand('S2V8') self.returnEntry.insert(10, returned) elif event.x >= 104 and event.x < 126: returned = BFR4WDserialport.sendcommand('S2V10') self.returnEntry.insert(10, returned) #Forward bar clicked if event.x > 133 and event.x < 170: if event.y >= 5 and event.y < 38: returned = BFR4WDserialport.sendcommand('W1D50') self.returnEntry.insert(10, returned) if event.y >= 38 and event.y < 70: returned = BFR4WDserialport.sendcommand('W1D30') self.returnEntry.insert(10, returned) if event.y >= 70 and event.y < 102: returned = BFR4WDserialport.sendcommand('W1D10') self.returnEntry.insert(10, returned) if event.y >= 102 and event.y < 133: returned = BFR4WDserialport.sendcommand('W1D5') self.returnEntry.insert(10, returned) #Reverse bar clicked if event.y >= 170 and event.y < 204: returned = BFR4WDserialport.sendcommand('W2D5') self.returnEntry.insert(10, returned) if event.y >= 204 and event.y < 235: returned = BFR4WDserialport.sendcommand('W2D10') self.returnEntry.insert(10, returned) if event.y >= 235 and event.y < 268: returned = BFR4WDserialport.sendcommand('W2D30') self.returnEntry.insert(10, returned) if event.y >= 268 and event.y < 300: returned = BFR4WDserialport.sendcommand('W2D50') self.returnEntry.insert(10, returned) #Turn ACW bar clicked if event.y > 134 and event.y < 172: if event.x >= 5 and event.x < 38: returned = BFR4WDserialport.sendcommand('W3D120') self.returnEntry.insert(10, returned) if event.x >= 38 and event.x < 70: returned = BFR4WDserialport.sendcommand('W3D90') self.returnEntry.insert(10, returned) if event.x >= 70 and event.x < 102: returned = BFR4WDserialport.sendcommand('W3D45') self.returnEntry.insert(10, returned) if event.x >= 102 and event.x < 133: returned = BFR4WDserialport.sendcommand('W3D30') self.returnEntry.insert(10, returned) #Turn CW bar clicked if event.x >= 170 and event.x < 204: returned = BFR4WDserialport.sendcommand('W4D30') self.returnEntry.insert(10, returned) if event.x >= 204 and event.x < 235: returned = BFR4WDserialport.sendcommand('W4D45') self.returnEntry.insert(10, returned) if event.x >= 235 and event.x < 268: returned = BFR4WDserialport.sendcommand('W4D90') self.returnEntry.insert(10, returned) if event.x >= 268 and event.x < 300: returned = BFR4WDserialport.sendcommand('W4D120') self.returnEntry.insert(10, returned)
def headClick(self, event): self.returnEntry.delete(0, END) # Speed select bar clicked if event.y > 10 and event.y < 40: if event.x >= 5 and event.x < 30: returned = BFR4WDserialport.sendcommand('S3V2') self.returnEntry.insert(10, returned) elif event.x >= 30 and event.x < 55: returned = BFR4WDserialport.sendcommand('S3V4') self.returnEntry.insert(10, returned) elif event.x >= 55 and event.x < 79: returned = BFR4WDserialport.sendcommand('S3V6') self.returnEntry.insert(10, returned) elif event.x >= 79 and event.x < 104: returned = BFR4WDserialport.sendcommand('S3V8') self.returnEntry.insert(10, returned) elif event.x >= 104 and event.x < 126: returned = BFR4WDserialport.sendcommand('S3V10') self.returnEntry.insert(10, returned) #Head tilt up clicked if event.x > 133 and event.x < 170: if event.y >= 5 and event.y < 38: returned = BFR4WDserialport.sendcommand('H1T75') self.returnEntry.insert(10, returned) if event.y >= 38 and event.y < 70: returned = BFR4WDserialport.sendcommand('H1T50') self.returnEntry.insert(10, returned) if event.y >= 70 and event.y < 102: returned = BFR4WDserialport.sendcommand('H1T25') self.returnEntry.insert(10, returned) if event.y >= 102 and event.y < 133: returned = BFR4WDserialport.sendcommand('H1T10') self.returnEntry.insert(10, returned) #Centralise head pan and tilt if central section is clicked if event.y >= 133 and event.y < 170: returned = BFR4WDserialport.sendcommand('H1T0P0') self.returnEntry.insert(10, returned) #Head tilt down clicked if event.y >= 170 and event.y < 204: returned = BFR4WDserialport.sendcommand('H1T-10') self.returnEntry.insert(10, returned) if event.y >= 204 and event.y < 235: returned = BFR4WDserialport.sendcommand('H1T-25') self.returnEntry.insert(10, returned) if event.y >= 235 and event.y < 268: returned = BFR4WDserialport.sendcommand('H1T-50') self.returnEntry.insert(10, returned) if event.y >= 268 and event.y < 300: returned = BFR4WDserialport.sendcommand('H1T-75') self.returnEntry.insert(10, returned) #Head pan ACW clicked if event.y > 134 and event.y < 172: if event.x >= 5 and event.x < 38: returned = BFR4WDserialport.sendcommand('H1P-75') self.returnEntry.insert(10, returned) if event.x >= 38 and event.x < 70: returned = BFR4WDserialport.sendcommand('H1P-50') self.returnEntry.insert(10, returned) if event.x >= 70 and event.x < 102: returned = BFR4WDserialport.sendcommand('H1P-25') self.returnEntry.insert(10, returned) if event.x >= 102 and event.x < 133: returned = BFR4WDserialport.sendcommand('H1P-10') self.returnEntry.insert(10, returned) #Head pan CW clicked if event.x >= 170 and event.x < 204: returned = BFR4WDserialport.sendcommand('H1P10') self.returnEntry.insert(10, returned) if event.x >= 204 and event.x < 235: returned = BFR4WDserialport.sendcommand('H1P25') self.returnEntry.insert(10, returned) if event.x >= 235 and event.x < 268: returned = BFR4WDserialport.sendcommand('H1P50') self.returnEntry.insert(10, returned) if event.x >= 268 and event.x < 300: returned = BFR4WDserialport.sendcommand('H1P75') self.returnEntry.insert(10, returned)
def sendCommand(self,event): returned = BFR4WDserialport.sendcommand(self.commandEntry.get()) self.returnEntry.delete(0,END) self.returnEntry.insert(10,returned) self.commandEntry.delete(0,END)
def servoOff(self, event): returned = BFR4WDserialport.sendcommand('S1V0') self.returnEntry.delete(0, END) self.returnEntry.insert(10, returned)
data = [xrobot, yrobot, thetaP, thetaT, sonar] #form data list print data writetofile(filename,data) #write data list to file ######################################################################################## # # Main # ######################################################################################## f = open('RawMapData.txt', 'w') #Open a file Initialise(10,10) Heading = BFR4WDserialport.sendcommand("G8") #get initial robot heading SonarArray = SonarSweep(-60, 60, 10, 0,f) returned = BFR4WDserialport.sendcommand("W1 D50") #Forward 50cm FLdistance = BFR4WDserialport.sendcommand("G1") RLdistance = BFR4WDserialport.sendcommand("G2") FRdistance = BFR4WDserialport.sendcommand("G3") RRdistance = BFR4WDserialport.sendcommand("G4") averageddistance = (int(FLdistance) + int(RLdistance) + int(FRdistance) + int(RRdistance))/4 Headingrads = math.radians(float(Heading)) xrobot = xrobot + int(math.sin(Headingrads) * averageddistance) yrobot = yrobot + int(math.cos(Headingrads) * averageddistance) #calculate new robot x and y coordinates SonarArray = SonarSweep(-60, 60, 10, 0,f)
def Initialise(wheelSpeed, headSpeed): returned = BFR4WDserialport.sendcommand("S1 V1") #Servo power on returned = BFR4WDserialport.sendcommand("S2 V" + str(wheelSpeed)) returned = BFR4WDserialport.sendcommand("S3 V" + str(headSpeed))
# Main # ######################################################################################## while True: print "Mode options: M = manual mode. A = Move and Look. B = Square. S = Sonar Scan. T = Test, I = Image" mode = raw_input("Mode: ") if mode == 'M': manualmode() elif mode == 'A': time.sleep(3) RunSequence([ "S1V1", "S2 V8", "S3 V4", "H1 P0 T0", "W1 D300", "H1 P-80", "W1 D300", "W2 D300", "S1 V25", "H1 P0", "W1 D400", "S1V0" ]) elif mode == 'B': time.sleep(3) RunSequence([ "S1V1", "S2 V15", "S3 V3", "W1 D200", "W3 D160", "W1 D200", "W3 D160", "W1 D200", "W3 D160", "W1 D200", "W3 D160", "S1V0" ]) elif mode == 'S': SonarArray = SonarScan() print SonarArray elif mode == 'T': RunFileSequence('BFRCode/Testfile') elif mode == 'I': BFR4WDOpenCV.DisplayFrame() BFR4WDserialport.closeserial()