def __calcluateTurn(self, tacho):
pose = Pose()
# the radian during this turn
deltaTheta = self.__calcluateRadianFromMotorTacho(tacho)
self.pose.theta -= deltaTheta
pose.x = self.pose.x
pose.y = self.pose.y
pose.theta = self.pose.theta%(math.pi*2)
return pose
def __calcluateMove(self, tacho):
pose = Pose()
moveDistance = self.__calcluateMoveDistanceFromTacho(tacho)
pose.x, pose.y = self.__getCoord(self.pose.x, self.pose.y, self.pose.theta, moveDistance)
pose.theta = self.pose.theta
return pose
