I am using a differential wheel robot for my project. I need to know the current coordinates of the robot with respect to it's initial position taken as the origin. I m doing the computation on an Arduino UNO and the only sensory input that I get is from the two encoders. I have the function updateOdomenty() called in the loop() and this is it's corresponding code:

void updateOdometry()

  static int encoderRPosPrev = 0;
  static int encoderLPosPrev = 0;

  float SR = distancePerCount * (encoderRPos - encoderRPosPrev);
  float SL = distancePerCount * (encoderLPos - encoderLPosPrev);

  encoderRPosPrev = encoderRPos;
  encoderLPosPrev = encoderLPos;

  x += SR * cos(theta);           
  y += SL * sin(theta);
  theta += (SR - SL) / wheelDistance;

  if(theta > 6.28)
    theta -= 6.28;
  else if(theta < -6.28)
    theta += 6.28;

This is the code that me and my team mates made after reading this paper. I am wondering if this is the best possible way to solve this problem with an Arduino. If not, how is odometry done in differential wheeled systems?

  • $\begingroup$ Oh just a quick comment - instead of using "6.28" I think you can use "2*PI" instead in Arduino language. If you don't do this you might have a bug if the heading angle falls between these vaues. $\endgroup$
    – Andy
    Commented Dec 18, 2015 at 10:10

1 Answer 1


I would add a few lines after you check that theta is between +/- 2pi:

meanDistance = (SL + SR)/2;
posX = posX + meanDistance*cos (theta);
posY = posY + meanDistance*sin(theta);

This of course assumes theta is positive CCW starting from the +x-axis. This is similar but not the same as your code for X and Y, but your code appears to put the X origin on the right wheel and Y origin on the left, and I'm not sure why you would do that.

Additionally, your code for theta uses a small angle sine approximation, sin(theta) = theta = dy/dL. You could use a true asin for this instead.

Otherwise, taking my comments above into account, this is how I would do differential-drive dead reckoning. Your next, least-expensive upgrade would be a compass or magnetometer, but that would probably come with a decent IMU, which would be your second upgrade, with GPS being the last. But, as I said, if all you need is dead reckoning, you've basically got it.

  • $\begingroup$ I am analyzing what you said. For one thing, the previous encoder values are initialized to zero only at the first function call as they are static. $\endgroup$ Commented Dec 16, 2015 at 20:13
  • $\begingroup$ @daltonfury42 - You are correct about the static function call and I am not a C programmer. Regarding my comment about your choice of origins, consider a zero point turn, such that SL = - SR. In this case, my code, (SL + SR)/2, gives a distance traveled of zero, which then updates X and Y with no change. Your code would actually move X a distance of SRcos(theta) and actually move Y a distance of SLsin(theta), as there are non-zero distances traveled by each wheel. $\endgroup$
    – Chuck
    Commented Dec 17, 2015 at 1:23
  • $\begingroup$ You said "all you need is dead reckoning", my aim is to navigate to a certain coordinate from the current coordinate. So this dead reconing function would be a first step. Am I going in the right direction? $\endgroup$ Commented Dec 17, 2015 at 6:13
  • $\begingroup$ "Am I going in the right direction?" (literally!) - yes, dead reckoning is the general phrase for keeping track of position relative to the start, by accounting for known velocity and direction. "Odometry" is just one way of doing this. Adding in compass/IMU, etc. as Chuck says allows more refined dead reckoning. What you've got so far looks basically correct to me. $\endgroup$
    – Andy
    Commented Dec 17, 2015 at 11:39
  • 1
    $\begingroup$ @BenyaminJafari - wheelDistance is the distance between the left and right wheels; it's called "track" on this Wikipedia page. $\endgroup$
    – Chuck
    Commented Feb 25, 2019 at 5:58

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.