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 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 __acquireCurrentPose(self):
poseAcquired = False
pose = ()
while not poseAcquired:
try:
pose = self.cam.getCurrentPose()
poseAcquired = True
break
except ValueError:
print "rotating of 5 degrees..."
self.robotMover.doRelativeRotation(5)
Robot.setCurrentPose(pose)
print "Current robot pose is: " + str(Robot.getCurrentPose())
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 searchSignal(self):
robotMover = RobotMover()
robotMover.doSnakeMovement(Terrain.DRAWING_ZONE_NORTH_EAST_CORNER_INNER, 180)
distance, signal = self.__doSignalSearch('SS.')
self.__adjustPosition(distance, 4)
signalPosition = Robot.getCurrentPosition()
self.__moveToSecondCorner()
distance, signal = self.__doSignalSearch('SB.')
self.__adjustPosition(distance, -4)
interpretedSignal = self.manchesterSignalInterpreter.interpretSignal(signal)
return interpretedSignal, signalPosition
def __doZignage_b(self):
print "Doing zignage B..."
self.captorsController.Zing_b()
Robot.setCurrentPose((Terrain.DRAWING_ZONE_CENTER[0], Terrain.DRAWING_ZONE_CENTER[1], 90))
def doRelativeRotation(self, relativeAngleInDegrees):
currentPose = Robot.getCurrentPose()
self.positionController.rotate(relativeAngleInDegrees)
Robot.setCurrentPose((currentPose[0], currentPose[1], currentPose[2] + relativeAngleInDegrees))
def __sendPoseToBase_rotate(self, angleInDegrees):
currentPose = Robot.getCurrentPose()
Robot.setCurrentPose((currentPose[0], currentPose[1], currentPose[2] + angleInDegrees))
def __adjustPosition(self, distance, verticalDrift):
ancientPose = Robot.getCurrentPose()
newPose = (ancientPose[0] - (int(distance)/10), ancientPose[1] + verticalDrift, ancientPose[2])
Robot.setCurrentPose(newPose)
