# 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?

## Answers 1

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

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.