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