Exemplo n.º 1
0
class RobotMover:
    def __init__(self):
        self.positionController = PositionController()

    def executeMoves(self, moves):
        for move in moves:
            if move.__module__ == advance.__name__:
                self.positionController.advance(move.distanceInCentimeters)
                self.__sendPoseToBase_advance(move.distanceInCentimeters)
            elif move.__module__ == rotate.__name__:
                self.positionController.rotate(move.angleInDegrees)
                self.__sendPoseToBase_rotate(move.angleInDegrees)
            elif move.__module__ == shuffle.__name__:
                self.positionController.shuffle(move.distanceInCentimeters, move.relativeAngleInDegrees)
                self.__sendPoseToBase_shuffle(move.distanceInCentimeters)

    def __buildPath(self, destination):
        pathBuilder = PathBuilder()
        path = pathBuilder.build(destination)

        print "path origin: " + str(path[0])

        del path[0]

        print "path without origin: " + str(path)

        return path

    def doSnakeMovement(self, destination, finalAbsoluteAngle):
        path = self.__buildPath(destination)

        moves = SnakeMovementPlanner().planMovement(Robot.getCurrentPose(), path, finalAbsoluteAngle)

        print "snake moves:"
        print moves

        self.executeMoves(moves)

        Robot.setCurrentPose((destination[0], destination[1], finalAbsoluteAngle))

    def doShuffleMovement(self, path, finalAbsoluteAngle):
        print "about to delete first node on shuffle path..."
        print "node to delete is: " + str(path[0])
        del path[0] # path[0] is the current robot's pose

        print "planning shuffle movement..."
        moves = ShuffleMovementPlanner().planMovement(Robot.getCurrentPose(), path)

        print "about to execute shuffle moves..."
        print moves

        self.executeMoves(moves)

        lastNode = path[len(path) - 1]
        Robot.setCurrentPose((lastNode[0], lastNode[1], finalAbsoluteAngle))

    def __sendPoseToBase_advance(self, distanceInCentimeters):
        currentPose = Robot.getCurrentPose()

        deltaY = math.sin(math.radians(currentPose[2])) * distanceInCentimeters
        deltaX = math.cos(math.radians(currentPose[2])) * distanceInCentimeters

        Robot.setCurrentPose((currentPose[0] + deltaX, currentPose[1] - deltaY, currentPose[2]))

    def __sendPoseToBase_rotate(self, angleInDegrees):
        currentPose = Robot.getCurrentPose()

        Robot.setCurrentPose((currentPose[0], currentPose[1], currentPose[2] + angleInDegrees))

    def __sendPoseToBase_shuffle(self, distanceInCentimeters):
        currentPose = Robot.getCurrentPose()

        deltaY = math.sin(math.radians(currentPose[2])) * distanceInCentimeters
        deltaX = math.cos(math.radians(currentPose[2])) * distanceInCentimeters

        Robot.setCurrentPose((currentPose[0] + deltaX, currentPose[1] - deltaY, currentPose[2]))

    def relativeShuffle(self, distance, relativeAngle):
        currentPose = Robot.getCurrentPose()
        currentAngle = currentPose[2]
        B = 360 - currentAngle - relativeAngle

        deltaX = distance * math.sin(math.radians(B))
        deltaY = distance * math.cos(math.radians(B))

        destination = (currentPose[0] + deltaX, currentPose[1] + deltaY, currentAngle)

        self.doShuffleMovement([currentPose, destination], currentAngle)

    def doRelativeRotation(self, relativeAngleInDegrees):
        currentPose = Robot.getCurrentPose()

        self.positionController.rotate(relativeAngleInDegrees)

        Robot.setCurrentPose((currentPose[0], currentPose[1], currentPose[2] + relativeAngleInDegrees))
Exemplo n.º 2
0
from python.src.robot.arduino.positioncontroller import PositionController

positionController = PositionController()
positionController.advance(10)
positionController.rotate(90)
positionController.advance(10)
positionController.rotate(-90)
positionController.advance(20)
positionController.rotate(180)