예제 #1
0
    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)
예제 #2
0
    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))