# How to perform odometry on an arduino for a differential wheeled robot?

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?

• 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. – Andy Dec 18 '15 at 10:10

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.

• 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. – daltonfury42 Dec 16 '15 at 20:13
• @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. – Chuck Dec 17 '15 at 1:23
• 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? – daltonfury42 Dec 17 '15 at 6:13
• "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. – Andy Dec 17 '15 at 11:39
• @BenyaminJafari - wheelDistance is the distance between the left and right wheels; it's called "track" on this Wikipedia page. – Chuck Feb 25 at 5:58