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