def performCalibration(color):
config.StiffnessOn(motionProxy)
config.calibrationPoseInit(motionProxy)
found = findNXT(color)
tts = ALProxy("ALTextToSpeech")
if(found):
tts.say('Start to calculate distance')
allMarker = detectMarkerAndCalcDist(IP, PORT, 10)
#print allMarker
directDistance = int(allMarker[0][4][0])
x = int(allMarker[0][4][1])
y = int(allMarker[0][4][2])
orientation = markerPosition[1][markerPosition[0].index(allMarker[0][3])]
print x, y, orientation
if(len(allMarker)>0):
tts.say('The nxt is '+ str(directDistance) + 'centimeter away from me!')
tts.say('The position of the NXT is ' + str(orientation))
return x, y, orientation
else:
tts.say('Error! Could not calculate distance. Make sure that the nxt didn\'t move')
myBroker.shutdown()
def changeBodyOrientation(self, orientation):
if(orientation == "init"):
self.bodyOrientation = "init"
self.naoCameraHeight = 50
config.walkPoseInit(self.motionProxy)
elif(orientation == "knee"):
self.bodyOrientation = "knee"
self.naoCameraHeight = 41
config.calibrationPoseInit(self.motionProxy)
elif(orientation == "zero"):
self.bodyOrientation = "zero"
self.naoCameraHeight = 52
config.poseZero(self.motionProxy)
def main():
n = NAOCalibration()
config.walkPoseInit(n.motionProxy)
config.calibrationPoseInit(n.motionProxy)
n.performCalibration(0) # search for red nxt
# print "------------------------------------------"
# n.performCalibration(1) #search for green nxt
# print "------------------------------------------"
# n.performCalibration(2) #search for blue nxt
# config.walkPoseInit(n.motionProxy)
sys.exit(1)
