Using an encoder and a gyro to find absolute displacement


I am working on a project that requires the robot’s displacement from the starting position to be known. The only useful sensors that I have are two encoders, one on each side of the robot, and a gyro. My general idea so far is that the encoders can be used to measure how far the wheels have spun. This can only be used to find displacement when driving in a straight line. I think the gyro could be used to correct for the turning of the wheels. I can also take multiple measurements along the path if needed.



What exactly is being asked here?


Assuming you mean the robot’s distance from the starting point, not the distance the robot has traveled, this would be my suggestion:

If you imagine the robot on a coordinate plane starting at (0,0) and facing 0 degrees, each tick of the code you could update the position using the distance traveled and the angle.

For example, if you track that you’ve moved 2 inches forward in the last tick while facing 0 degrees you could update your coordinates to (2,0) and update the angle. The next tick if you moved 3 inches forward but you were facing 15 degrees you’d take your current X and add 3cos(15) and take your current Y and add 3sin(15) and that’d be your new position in the coordinate plane. Then to take the distance traveled from the origin it’d simply be the Pythagorean theorem.


This is a process that is used by many teams and is commonly referred to as “odometry” or “state estimation”. You can basically calculate the distance traveled by the robot at a fixed interval by averaging your left and right encoders to get d. If your dt is small enough, you can assume the robot goes in straight lines and turns. Your gyro angle would then be your theta. Use vector addition from the initial pose to calculate your position on the field.

<d cos theta, d sin theta> + previous pose = current pose.

Some teams like 254 use differential geometry to represent the robot’s movement as movement along an arc of constant curvature at each dt interval. More info can be found here:

Be mindful that this method of finding your position on the field is usually only accurate for the first 15 seconds or so. Wheel slip, encoder slip, and other factors will cause this approximation to deviate significantly after a while.


Like Prateek said, to get the distance you just do

(leftEncoder + rightEncoder) / 2

And to make sure it stays straight, you can use the gyro and a very simple PID function like this:

speed = 1 // 100%
turnAmount = (DESIRED_ANGLE - gyro) / -10 // in degrees // the PID part with P

leftSpeed = speed * (1 - (turnAmount < 0 ? -turnAmount : 0));
rightSpeed = speed * (1 - (turnAmount > 0 ?  turnAmount : 0));
// OR you can just do this:
leftSpeed = speed * (1 + min(turnAmount, 0));
rightSpeed = speed * (1 - max(turnAmount, 0));


Depending on how much slippage occurs, “a while” could be on the order of a few seconds. It does really depend.

I don’t know of any team that builds control strategies around the assumption that they will know absolute position at any given time.

This isn’t to say you shouldn’t pursue how to get this answer - simply to make sure you level-set your expectations.