def findNXT(self, NXTColor):
# set the initial head position for NAO
config.setHeadMotion(self.motionProxy, 0, 0)
# move head vertical
for g in range(0, len(self.pitchIntervals)):
# move head horizontal
for h in range(0, len(self.yawIntervals)):
# set the head position to the current yaw and pitch interval
config.setHeadMotion(self.motionProxy, self.yawIntervals[h], self.pitchIntervals[g])
# make 5 measurements to be sure that there is / is no marker detected
self.detectMarker(5)
# calculate the averages for all detected markers
self.calcAvgOfAllDetectedMarker()
found = self.centerHeadToMarkerToDetectColor(NXTColor)
if found != -1:
print "NXT with color" + str(self.colors[NXTColor]) + " found!"
self.textToSpeechProxy.say("NXT with color" + str(self.colors[NXTColor]) + " found!")
return True
print "NXT " + str(NXTColor) + " not found!"
self.textToSpeechProxy.say("NXT " + str(NXTColor) + " not found!")
return False
def main():
print "-----------------begin:NAOCalibration-----------------------"
n = NAOCalibration()
n.changeBodyOrientation("init")
print "---------------------look for red NXT-----------------------"
n.performCalibration(0) # red
print "---------------------look for green NXT---------------------"
n.performCalibration(1) # green
print "---------------------look for blue NXT----------------------"
n.performCalibration(2) # blue
config.setHeadMotion(n.motionProxy, 0, 0)
n.changeBodyOrientation("knee")
print "-------------------end:NAOCalibration-----------------------"
sys.exit(1)
def centerHeadToMarkerWithColor(self, NXTColor):
currentHeadPosition = self.getHead()
for i in range(0, len(self.allDetectedMarker)):
config.setHeadMotion(self.motionProxy, self.toDEG(currentHeadPosition[0]), self.toDEG(currentHeadPosition[1]))
# only calculate if there is one marker
if(self.allDetectedMarker[i][0] != []):
avgAlpha = self.allDetectedMarkerAVG[i][0]
avgBeta = self.allDetectedMarkerAVG[i][1]
self.printAndSayMessage("Centre head to marker "+ str(self.allDetectedMarkerAVG[i][3])+ " to get the color")
config.setHeadMotion(self.motionProxy, self.toDEG(self.getHead()[0]+avgAlpha), self.toDEG(self.getHead()[1]+avgBeta))
self.calcAvgOfAllDetectedMarker()
time.sleep(0.7)
if(colourDetection.getColour(self.IP, self.PORT, self.allDetectedMarkerAVG[i][2]) == NXTColor):
# head is centered to the right marker with the color of nxt
return i
return -1
def centerHeadToMarkerToDetectColor(self, NXTColor):
currentHeadPosition = self.getHead()
for i in range(0, len(self.allDetectedMarker)):
config.setHeadMotion(
self.motionProxy, self.toDEG(currentHeadPosition[0]), self.toDEG(currentHeadPosition[1])
)
# only calculate if there is one marker
if self.allDetectedMarker[i][0] != []:
print str(i) + " "
avgAlpha = self.allDetectedMarkerAVG[i][0]
avgBeta = self.allDetectedMarkerAVG[i][1]
print "head centred to one detected marker to get the color"
config.setHeadMotion(
self.motionProxy, self.toDEG(self.getHead()[0] + avgAlpha), self.toDEG(self.getHead()[1] + avgBeta)
)
if colourDetection.getColour(self.IP, self.PORT, 320, 240) == NXTColor:
# head is centered to the right marker with the color of nxt
return i
return -1
def findColouredMarker(self, NXTColor):
# set the initial head position for NAO
config.setHeadMotion(self.motionProxy, 0, 0)
interval = 1
# for all cameras on NAO
for i in range(0, 2):
# 0 is topCamera, 1 is bottomCamera
self.changeCurrentCamera(i)
# for all pitchIntervals move head vertically
for g in range(0, len(self.pitchIntervals[i])):
# for all yawIntervals move head horizontal
for h in range(0, len(self.yawIntervals)):
self.printConsole("interval: ", str(interval)+"/"+str(len(self.pitchIntervals[i])* len(self.pitchIntervals) * len(self.yawIntervals)))
# set the head position to the current yaw and pitch interval
config.setHeadMotion(self.motionProxy, self.yawIntervals[h], self.pitchIntervals[i][g])
#make 5 measurements to be sure that there is / is no marker detected
self.detectMarker(5)
# calculate the averages for all detected markers
self.calcAvgOfAllDetectedMarker()
# now we prove for every found maker if it has the right color
colorIndex = self.centerHeadToMarkerWithColor(NXTColor)
self.printConsole("colorIndex", colorIndex)
if(colorIndex != -1):
self.printAndSayMessage('NXT with color ' + str(self.colors[NXTColor]) + ' found!')
return interval
interval=interval+1
return -1