def startSequence(self):
currentNumber = 1
while True:
# Wait until the correct signal is received
self.waitUntilBaseStationIsReady()
self.initializeVariables()
# Rotate to be ready to measure the resistance value
self.rotateForOhmmeter()
if currentNumber == 1:
self.findObstacles()
resistanceColorCode = self.moveAndMeasureResistance()
letterCommandPanelValue, _ = resistanceColorCode[0]
directionCommandPanelValue, _ = resistanceColorCode[2]
self._moveFromRobotToCoordinates(self.STARTING_ZONE_CENTER, dilatation=NO_DILATATION)
# Find command from letter command panel
self.moveToLetterCommandPanel()
letterCommand = self.findCommand(self.letterCommandFinder, letterCommandPanelValue)
self.communicator.sendFirstCommandPanelCommand(letterCommand.upper())
print "Commande panneau lettre: ", letterCommand
# Find command from direction command panel
self.moveToDirectionCommandPanel()
directionCommand = self.findCommand(self.directionCommandFinder, directionCommandPanelValue)
if directionCommand == "_":
print "Erreur lecture panneau"
for _ in range(self.NUMBER_OF_RETRIES):
try:
self._move(0, -50)
image = self.imageTaker.getImage()
orientationFinder = LogitechOrientationFinder(image)
angleToRotate = orientationFinder.findRotation(45) * -1
print "Angle correction: ", angleToRotate
self.nextExpectedOrientation = 0
self.locomotionSystem.rotate(angleToRotate)
break
except (ValueError, TypeError):
self._move(0, -150)
else:
exit("Échec de trouver une ligne")
directionCommand = self.findCommand(self.directionCommandFinder, directionCommandPanelValue)
self.communicator.sendSecondCommandPanelCommand(directionCommand.upper())
print "Commande panneau direction: ", directionCommand
# Find which puck to bring to which corner
puckColors = [color for (value, color) in resistanceColorCode]
finalPuckPositionFinder = FinalPucksPositionFinder()
self.finalPucksPosition = finalPuckPositionFinder.findFinalPucksPosition(letterCommand, directionCommand, puckColors)
print "Pucks: ", self.finalPucksPosition
self.transformationToFindPucks = TRANSFORMATIONS_FROM_YAWPITCH[(0,-45)]
self.prepareCameraForPucks()
self.grabAllPucks(self.finalPucksPosition)
time.sleep(1)
self.internCommunicator.turnOnEndingLED()
self.communicator.sendEndSignal()
currentRun += 1
print "Fin de la sequence"
time.sleep(3)
def grabAllPucks(self, finalPucksPosition):
pucksToGrab = list(finalPucksPosition.keys())
self._moveFromRobotToCoordinates(self.CENTER_PUCK_ZONE)
puckOrientations = self.findPuckOrientations(pucksToGrab)
print puckOrientations
lastMovement = len(puckOrientations.items()) - 1
pucksToGrabInOrder = self.getGrabOrder(finalPucksPosition)
for i, puck in enumerate(pucksToGrabInOrder):
orientation = puckOrientations[puck]
print "Puck: ", str(puck.outerColor)
for offset in (0, -1, 1):
index = self.anglesToCheck.index(orientation) + offset
index = max(index, 0)
index = min(index, len(self.anglesToCheck) - 1)
robotOrientation = self.anglesToCheck[index]
self._rotateToAngle(robotOrientation)
try:
x, y = self.findPuckCoordinates(puck, TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)])
break
except PuckNotFoundException:
pass
else:
exit("Échec lamentable")
for _ in range(self.NUMBER_OF_RETRIES):
self._move(x * self.MILLIMETERS_PER_CENTIMETER, 0)
self._move(0, y * self.MILLIMETERS_PER_CENTIMETER)
self.grabPuck()
self._move(0, -(y * self.MILLIMETERS_PER_CENTIMETER - self.HALF_PUCK_SIZE_IN_MILLIMETERS))
try:
x, y = self.findPuckCoordinates(puck, TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)])
print "Puck not grabbed"
continue
except PuckNotFoundException:
print "Puck grabbed"
break
else:
exit("Échec de prendre la rondelle")
self._moveFromRobotToCoordinates(self.CENTER_PUCK_ZONE)
self._rotateToAngle(0)
self.internCommunicator.rotateCamera("pitch", -45)
for _ in range(self.NUMBER_OF_RETRIES):
try:
image = self.imageTaker.getImage()
orientationFinder = LogitechOrientationFinder(image)
angleToRotate = orientationFinder.findRotation(45) * -1
print "Angle correction: ", angleToRotate
self.nextExpectedOrientation = 0
self.locomotionSystem.rotate(angleToRotate)
break
except (ValueError, TypeError):
self._move(0, -150)
else:
exit("Échec de trouver une ligne")
self._moveFromRobotToCoordinates(self.STARTING_ZONE_CENTER, forceOrientation=0)
self._moveFromRobotToCoordinates(self.STARTING_ZONE_CENTER)
self.moveAndDropPuck(finalPucksPosition[puck], TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)], lastMovement=(i == lastMovement))