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 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 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)
def GetIR():
IRArray = []
IRArray.append(int(BFR4WDserialport.sendcommand("G6")))
IRArray.append(int(BFR4WDserialport.sendcommand("G7")))
return IRArray
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))
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()
