Odometry Turning Help

I have been working on an Odometry system over the summer. I got it to accurately track position and combined it with my existing PID. When driving straight it works great but when turning in place the encoder values think the robot is moving position. To try and solve this I did the circumference of the robot * deltaTheta(deg)/360. Then I divided that by the wheel diameter

//DeltaTheta is in radians
//convert to degrees then divide by the circumference of the whole robot


This is just a snippet of the code since that is the only part I was adjusting. For context since I was using my old PID the global position is in degrees not inches. Inches are the input for most things and then I convert it to degrees. This is also part of my greater odomtrey task that runs every 10 milliseconds. I’m using one perpendicular wheel, one parallel, and one IMU

I wanted to know if there is a better way to solve for turning in place with odometry because currently it works when turning to the right(positive) but not negative. When going left it drops out of bounds from 400 degrees (10ish inches) to 100000 degrees. I also noticed that since this test bot was not built well the inertial sensor is subject to drift especially when first initializing (I knew this, but it doesn’t affect it too much) except when adding the turning offset calculations the robot starts with its position in a crazy error till I turn it to the right a degree.

The last issue I’ve noticed which is likely due partially to how I’m testing this is if I turn the robot from 0 to anything above 90 there is a lot of more difference between the actual position and what it reads out. This though is likely caused by mechanics since I’m using mecanum wheels and they are poorly balanced.
For instance, turning 270 degrees yields a final position of -118 degrees x and -428-degrees y. Which might be correct, but I’ll double check that portion another time manually.

Thanks in advance for any help. Also, if this has already been answered feel free to direct me to it.

1 Like


if I understand the problem, you want to use PID to turn on the place.

I read you use an inertial sensor, which makes turning on the place easier.

In my case, I use these two formulas to calculate the error

alpha_error = desire_turn_in_deg - (int)inertial.get_heading();
gama_error = desire_turn_in_deg - (int)inertial.get_heading() - 360;

I checked which result is closer to 0 in absolute value.
If the closer absolute valued result was negative the closer way is to turn to left
If positive closer way is to turn to right

ex. desire angle 270, current angle 0
alpha_error = 270
gama_error = -90

if I turn both numbers into absolute values, the closer number to 0 is 90 than 270
and 90 was negative so closer will be turning 90 deg to the left

I can send the code to you; however, it is in pros

just write me


I appreciate the help.

This is on me I didn’t do a good job explaining my issue. Essentially I created a system to track my global coordinates using two encoders(one in the x and one in the y).
I know at some point there might be a case where I don’t want to move but just turn. I have my turning pid configured for the inertial sensor but my tracking system didn’t know the robot wasn’t moving. So prior to the snippet of code I posted the coordinate system when turning the robot 90 degrees would think yes angle did change but the coordinate if you were at 1,1 would read something like 29,34.
So the code I had originally posted calculates in the amount you turned so the coordinate system reads correctly.

Odometry runs separately from the pids (I have a few. So when I said it was in degrees I meant my distance pid for slowing to a point. I forgot to specify. )

This worked except tracking when the robot turns left. So my main question was is their a way to solve that and 2. Is there just a different way to calculate this turning offset. Though the latter is more out of curiosity.

Based on how my coordinates go crazy when tracking a left turn I assume it’s one of few things:1. The encoders are going the other direction than they are for right turns so the math needs to be slightly changed,2. There is a case where division or some other math is doing something weird like dividing by zero, 3. The sign of the turn offset needs to changed when turning left.

Do you only have one encoder on the side of the drivetrain? I believe you need two for accurate Odometry, plus a third in the back or an IMU. Could be the source of the problem

This is close. The OP could add an IMU to give the heading. Alternatively, having the 2 current tracking wheels placed parallel to the drivetrain’s forward direction and lined up would work. The third, orthogonal tracking wheel accounts for lateral and is necessary if the drivetrain is an x drive and can improve accuracy in a skid steer (aka tank) drivetrain

The OP seems to have the 2 tracking wheels orthogonal to eachother, but using code to compute change in heading that assumes they are parallel from eachother. I don’t think the current configuration would allow one to compute change in heading


I understand the problem
This formula should help

Can you send whole code on the end, please?

It will be helpful

Ps. For the odometry, I still recomend one parallel and one horizontal tracking wheel + IMU cause you can be still pushed by other robots sideways


So, I actually realized the issue is likely a fault in my logic. I was unable to test most of the week, but upon further testing I discovered the error I’m getting with position occurs specifically at the inertial sensor’s rollover from 360 to 0. The direction does not matter. Doing some example calculations on paper it occurred to me that the change in theta gets messed up since one moment it is something like 350 then it hits 2 or vice versa. This makes deltaTheta something like -340 or 340 depending on direction and the odometry tries to correct for such a big jump by changing the position drastically.

I’m trying to think how to fix it but I haven’t quite thought of a way yet. Thanks for any help with this.

double arc=ConvertToDeg(deltaTheta)/360*(M_PI*18);
double turnOffsetx = (arc*360/(M_PI*3.25));
double turnOffsety = (arc*360/(M_PI*3.25));
 deltaR = ((Right.rotation(deg)-prevR))+tOY;
  deltaS = ((Back.rotation(deg)- prevS))-tOX;
  prevR = Right.rotation(deg);
  prevS = Back.rotation(deg);

    if (absoluteAngle <0) {
    if (absoluteAngle >360) {

    deltaTheta = absoluteAngle - prevTheta;

    prevTheta = absoluteAngle;

    if ((deltaTheta) <= .5 && deltaTheta >= -.5) {
      deltaXLocal = (deltaS);
      deltaYLocal = (deltaR);
      halfAngle = 0;
    } else {
      halfAngle = deltaTheta/2;
      r = deltaR/deltaTheta;
      r2 = deltaS/deltaTheta;
      deltaXLocal = 2*sin(halfAngle) *(r2+sDist);
      deltaYLocal = 2*sin(halfAngle) * (r-rDist);

  xPosGlobal += (deltaXGlobal);
  yPosGlobal += (deltaYGlobal);
return 1;

I’ll apologize in advance for the messiness I deleted a bunch of old comments and didn’t fix the spacing yet. Again this is just the snippet I think is relevant here since this is where most of the delta theta code is. I currently have my starting heading as 0.

I tried changing my theta to starting at pi and making it go counter-clockwise by doing (360-heading) then converting it to radians. I then measured theta for global x and y by substituting absolutetheta-deltatheta/2 in for absolute theta. However this had the same problem when hitting the change from .5 rad to 6.2 rad.

Is there a way to solve this or is one of those things you just avoid?

1 Like

The simplest way is use rotation() rather than heading().