def sendHeadSliders(self, event):
print "Sending Command", 'H0R' + str(
self.RollSlider.get()) + 'P' + str(
self.PitchSlider.get()) + 'Y' + str(self.YawSlider.get())
Desktop_Head_Arm_Serial.sendcommand('H0R' +
str(self.RollSlider.get()) + 'P' +
str(self.PitchSlider.get()) + 'Y' +
str(self.YawSlider.get()))
def UpdateSonar(self):
Desktop_Head_Arm_Serial.sendcommand('A5')
SonarDistance = int(Desktop_Head_Arm_Serial.readserial())
self.SonarCanvas.create_rectangle(0, 0, 200, 50, fill="black")
self.SonarCanvas.create_line(0,
25,
SonarDistance / 2,
25,
fill="green",
width=20)
self.SonarEntry.delete(0, END)
self.SonarEntry.insert(0, SonarDistance)
self.after(100, self.UpdateSonar)
def ArmTest(self):
Desktop_Head_Arm_Serial.sendcommand('A0 B0L0U-70G0')
time.sleep(1)
Desktop_Head_Arm_Serial.sendcommand('A0 B-36L89U-43G0')
time.sleep(1)
Desktop_Head_Arm_Serial.sendcommand('A0 B-36L99U-35G10')
time.sleep(1)
Desktop_Head_Arm_Serial.sendcommand('A0 B6L10U46G-45')
time.sleep(1)
def sendArmSliders(self, event):
print "Sending Command", 'A0B' + str(
self.RotateSlider.get()) + 'L' + str(
self.LowerSlider.get()) + 'U' + str(self.ElbowSlider.get())
Desktop_Head_Arm_Serial.sendcommand('A0B' +
str(self.RotateSlider.get()) +
'L' + str(self.LowerSlider.get()) +
'U' + str(self.ElbowSlider.get()) +
'G' +
str(self.GripperSlider.get()))
if self.livedatastatus.get() == 0:
self.PlotRobotArm([
self.RotateSlider.get(),
self.LowerSlider.get(),
self.ElbowSlider.get(),
self.GripperSlider.get()
])
def ColourDetection(self):
print 'Colour detection'
if self.Colourdetectionstatus.get() == 1:
balldata, img, imgthreshed = Desktop_Head_Arm_OpenCV.FindBall(
GREENOBJECTS)
self.ImagetoGUI1(img)
self.ImagetoGUI2(imgthreshed)
if self.Colourtrackingstatus.get(
) == 1 and balldata is not -1: #tracking selected and object detected
print "Colour Tracking"
errorX = CamCentreX - balldata[0]
print errorX
self.YawSlider.set(self.YawSlider.get() - (errorX * 0.04))
#Eye tracking for x direction
if errorX > 80 and self.EyeStatus is not 1:
Desktop_Head_Arm_Serial.sendcommand(
'F0E1') #Happy face eyes left
self.EyeStatus = 1
elif errorX < -80 and self.EyeStatus is not 2:
Desktop_Head_Arm_Serial.sendcommand(
'F0E2') #Happy face eyes right
self.EyeStatus = 2
elif errorX < 80 and errorX > -80 and self.EyeStatus is not 0:
Desktop_Head_Arm_Serial.sendcommand('F0E0') #Happy face
self.EyeStatus = 0
errorY = CamCentreY - balldata[1]
print errorY
self.PitchSlider.set(self.PitchSlider.get() + (errorY * 0.04))
self.sendHeadSliders(self)
self.after(50, self.ColourDetection)
def FaceDetection(self):
print 'Face detection'
if self.Facedetectionstatus.get() == 1:
self.Facecoordinates, self.OpenCVImage = Desktop_Head_Arm_OpenCV.FindFaces(
)
print self.Facecoordinates
self.ImagetoGUI1(self.OpenCVImage)
self.ImagetoGUI2(self.OpenCVImage)
print len(self.Facecoordinates)
if len(self.Facecoordinates) is 1: #No Face detected
if self.FaceDetected is False:
self.FaceDetected = True
Desktop_Head_Arm_Serial.sendcommand('F2E0') #Sad face
elif len(self.Facecoordinates) is 2: #Face detected
if self.FaceDetected is True:
self.FaceDetected = False
Desktop_Head_Arm_Serial.sendcommand('F0E0') #Happy face
errorX = CamCentreX - self.Facecoordinates[1][0]
print errorX
self.YawSlider.set(self.YawSlider.get() - (errorX * 0.04))
#Eye tracking for x direction
if errorX > 80 and self.EyeStatus is not 1:
Desktop_Head_Arm_Serial.sendcommand(
'F0E1') #Happy face eyes left
self.EyeStatus = 1
elif errorX < -80 and self.EyeStatus is not 2:
Desktop_Head_Arm_Serial.sendcommand(
'F0E2') #Happy face eyes right
self.EyeStatus = 2
elif errorX < 80 and errorX > -80 and self.EyeStatus is not 0:
Desktop_Head_Arm_Serial.sendcommand('F0E0') #Happy face
self.EyeStatus = 0
errorY = CamCentreY - self.Facecoordinates[1][1]
print errorY
self.PitchSlider.set(self.PitchSlider.get() + (errorY * 0.04))
self.sendHeadSliders(self)
self.after(50, self.FaceDetection)
else: #If face detection is disabled, send a happy face command to arduino
Desktop_Head_Arm_Serial.sendcommand('F0E0') #Happy face
def HeadNod(self):
#Head nod
Desktop_Head_Arm_Serial.sendcommand('F0E0')
Desktop_Head_Arm_Serial.sendcommand('H0R0P20Y0')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P-20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P-20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P-20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P20')
def HeadShake(self):
#Head shake
Desktop_Head_Arm_Serial.sendcommand('F3E0')
Desktop_Head_Arm_Serial.sendcommand('H0R0P0Y20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P0Y-20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P0Y20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P0Y-20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P0Y20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P0Y-20')
time.sleep(0.5)
Desktop_Head_Arm_Serial.sendcommand('H0R0P0Y20')
def Updateservopositions(self):
Desktop_Head_Arm_Serial.sendcommand('H1')
Rollservopos = int(Desktop_Head_Arm_Serial.readserial())
self.RollEntry.delete(0, END)
self.RollEntry.insert(0, Rollservopos)
Desktop_Head_Arm_Serial.sendcommand('H2')
Pitchservopos = int(Desktop_Head_Arm_Serial.readserial())
self.PitchEntry.delete(0, END)
self.PitchEntry.insert(0, Pitchservopos)
Desktop_Head_Arm_Serial.sendcommand('H3')
Yawservopos = int(Desktop_Head_Arm_Serial.readserial())
self.YawEntry.delete(0, END)
self.YawEntry.insert(0, Yawservopos)
Desktop_Head_Arm_Serial.sendcommand('A1')
Rotateservopos = int(Desktop_Head_Arm_Serial.readserial())
self.RotateEntry.delete(0, END)
self.RotateEntry.insert(0, Rotateservopos)
Desktop_Head_Arm_Serial.sendcommand('A2')
Lowerservopos = int(Desktop_Head_Arm_Serial.readserial())
self.LowerEntry.delete(0, END)
self.LowerEntry.insert(0, Lowerservopos)
Desktop_Head_Arm_Serial.sendcommand('A3')
Elbowservopos = int(Desktop_Head_Arm_Serial.readserial())
self.ElbowEntry.delete(0, END)
self.ElbowEntry.insert(0, Elbowservopos)
Desktop_Head_Arm_Serial.sendcommand('A4')
Gripperservopos = int(Desktop_Head_Arm_Serial.readserial())
self.GripperEntry.delete(0, END)
self.GripperEntry.insert(0, Gripperservopos)
if self.livedatastatus.get() == 1:
XYZSonar = self.PlotRobotArm([
Rotateservopos, Lowerservopos, Elbowservopos, Gripperservopos
])
if self.Plotworldstatus.get(
) == 1: #if we want to log sonar end positions
self.WorldArray.append(
[XYZSonar.item(0),
XYZSonar.item(1),
XYZSonar.item(2)])
self.DatapointEntry.delete(0, END)
self.DatapointEntry.insert(0, len(self.WorldArray))
self.after(100, self.Updateservopositions)
def sendCommand(self, event):
print "Sending Command", self.commandEntry.get()
Desktop_Head_Arm_Serial.sendcommand(self.commandEntry.get())
def armservopower(self):
print "Arm Servo Power is", self.armservocheckstatus.get()
if self.armservocheckstatus.get() is 0:
Desktop_Head_Arm_Serial.sendcommand('S3')
else:
Desktop_Head_Arm_Serial.sendcommand('S1')
def headservopower(self):
print "Head Servo Power is", self.headservocheckstatus.get()
if self.headservocheckstatus.get() is 0:
Desktop_Head_Arm_Serial.sendcommand('S2')
else:
Desktop_Head_Arm_Serial.sendcommand('S0')