class Odometry: def __init__(self): self.leftEncoder = Encoder() self.rightEncoder = Encoder() self.pose = Pose() self.lastTime = 0 # def setWheelSeparation(self, separation): # self.wheelSeparation = separation # def setTicksPerMeter(self, ticks): # self.ticksPerMeter = ticks # def setEncoderRange(self, low, high): # self.leftEncoder.setRange(low, high) # self.rightEncoder.setRange(low, high) # def setTime(self, newTime): # self.lastTime = newTime # def updateLeftWheel(self, newCount): # self.leftEncoder.update(newCount) # def updateRightWheel(self, newCount): # self.rightEncoder.update(newCount) def updatePose(self, newTime): leftTravel = self.leftEncoder.getDelta() / self.ticksPerMeter rightTravel = self.rightEncoder.getDelta() / self.ticksPerMeter deltaTime = newTime - self.lastTime deltaTravel = (rightTravel + leftTravel) / 2 deltaTheta = (rightTravel - leftTravel) / self.wheelSeparation if rightTravel == leftTravel: deltaX = leftTravel * cos(self.pose.theta) deltaY = leftTravel * sin(self.pose.theta) else: radius = deltaTravel / deltaTheta # Find the instantaneous center of curvature (ICC). iccX = self.pose.x - radius * sin(self.pose.theta) iccY = self.pose.y + radius * cos(self.pose.theta) deltaX = cos(deltaTheta)*(self.pose.x - iccX) \ - sin(deltaTheta)*(self.pose.y - iccY) \ + iccX - self.pose.x deltaY = sin(deltaTheta)*(self.pose.x - iccX) \ + cos(deltaTheta)*(self.pose.y - iccY) \ + iccY - self.pose.y self.pose.x += deltaX self.pose.y += deltaY self.pose.theta = (self.pose.theta + deltaTheta) % (2 * pi) self.pose.xVel = deltaTravel / deltaTime if deltaTime > 0 else 0. self.pose.yVel = 0 self.pose.thetaVel = deltaTheta / deltaTime if deltaTime > 0 else 0. self.lastTime = newTime def getPose(self): return self.pose def setPose(self, newPose): self.pose = newPose
class Odometry: """Keeps track of the current position and velocity of a robot using differential drive. """ def __init__(self): self.leftEncoder = Encoder() self.rightEncoder = Encoder() self.pose = Pose() self.lastTime = 0 def setWheelSeparation(self, separation): self.wheelSeparation = separation def setTicksPerMeter(self, ticks): self.ticksPerMeter = ticks def setEncoderRange(self, low, high): self.leftEncoder.setRange(low, high) self.rightEncoder.setRange(low, high) def setTime(self, newTime): self.lastTime = newTime def updateLeftWheel(self, newCount): self.leftEncoder.update(newCount) def updateRightWheel(self, newCount): self.rightEncoder.update(newCount) def updatePose(self, newTime): """Updates the pose based on the accumulated encoder ticks of the two wheels. See https://chess.eecs.berkeley.edu/eecs149/documentation/differentialDrive.pdf for details. """ leftTravel = self.leftEncoder.getDelta() / self.ticksPerMeter rightTravel = self.rightEncoder.getDelta() / self.ticksPerMeter deltaTime = newTime - self.lastTime deltaTravel = (rightTravel + leftTravel) / 2 deltaTheta = (rightTravel - leftTravel) / self.wheelSeparation if rightTravel == leftTravel: deltaX = leftTravel*cos(self.pose.theta) deltaY = leftTravel*sin(self.pose.theta) else: radius = deltaTravel / deltaTheta # Find the instantaneous center of curvature (ICC). iccX = self.pose.x - radius*sin(self.pose.theta) iccY = self.pose.y + radius*cos(self.pose.theta) deltaX = cos(deltaTheta)*(self.pose.x - iccX) \ - sin(deltaTheta)*(self.pose.y - iccY) \ + iccX - self.pose.x deltaY = sin(deltaTheta)*(self.pose.x - iccX) \ + cos(deltaTheta)*(self.pose.y - iccY) \ + iccY - self.pose.y self.pose.x += deltaX self.pose.y += deltaY self.pose.theta = (self.pose.theta + deltaTheta) % (2*pi) self.pose.xVel = deltaTravel / deltaTime if deltaTime > 0 else 0. self.pose.yVel = 0 self.pose.thetaVel = deltaTheta / deltaTime if deltaTime > 0 else 0. self.lastTime = newTime def getPose(self): return self.pose; def setPose(self, newPose): self.pose = newPose
class TestEncoder(unittest.TestCase): def setUp(self): self.encoder = Encoder() def testInitialization(self): self.assertEquals(self.encoder.getDelta(), 0) def testClearedDelta(self): self.encoder.update(100) self.assertEquals(self.encoder.getDelta(), 100) self.assertEquals(self.encoder.getDelta(), 0) def testIncrement(self): self.encoder.update(100) self.assertEquals(self.encoder.getDelta(), 100) self.encoder.update(50) self.assertEquals(self.encoder.getDelta(), -50) def testWraparound(self): defaultRange = 32767 - (-32768) + 1 self.encoder.update(20000) self.assertEquals(self.encoder.getDelta(), 20000) # Wrap around the high end. self.encoder.update(-20000) self.assertEquals(self.encoder.getDelta(), (-20000) + defaultRange - 20000) # Wrap around the low end. self.encoder.update(20000) self.assertEquals(self.encoder.getDelta(), 20000 - defaultRange - (-20000)) def testCustomRange(self): self.encoder.setRange(0, 999) self.encoder.update(500) self.assertEquals(self.encoder.getDelta(), 500) self.encoder.update(900) self.assertEquals(self.encoder.getDelta(), 400) # Wrap around the high end. self.encoder.update(100) self.assertEquals(self.encoder.getDelta(), 200) # Wrap around the low end. self.encoder.update(900) self.assertEquals(self.encoder.getDelta(), -200)