Odometry Turning Issue

My issue is that whenever I turn in place my robot position changes. I believe this is because the perpendicular wheel is moving and I haven’t found a fix yet. My robot uses an inertial sensor, one parallel wheel, and one perpendicular wheel. OdomRotationX is the perpendicular wheel.

    WheelTravelX = (((OdomRotationX.position(rotationUnits::rev)*360) * WheelSizeIn * M_PI)/360) - (cos(RobotAngle) * OdomXOff);
    WheelTravelY = (((OdomRotationY.position(rotationUnits::rev)*360) * WheelSizeIn * M_PI)/360) - (sin(RobotAngle) * OdomYOff);
    WheelTravelX2 = WheelTravelX - PreWheelTravelX;
    WheelTravelY2 = WheelTravelY - PreWheelTravelY;
    RobotAngle = abso(Inertial.rotation()) * (M_PI/180);
    LocalRobotPosX = cos(RobotAngle) * (WheelTravelX2) - sin(RobotAngle) * (WheelTravelY2);
    LocalRobotPosY = sin(RobotAngle) * (WheelTravelX2) - cos(RobotAngle) * (WheelTravelY2);

    RobotPositionX += LocalRobotPosX;
    RobotPositionY += LocalRobotPosY;

    PreWheelTravelX = WheelTravelX;
    PreWheelTravelY = WheelTravelY;

My solution would be to subtract (changeinAngle * strafe distance from center) from your distance calculated from the strafe wheel. This should subtract the arc length the wheel travels when you turn so it doesn’t affect your results anymore.

1 Like

Thank you I will try that when I get the chance.

That didn’t seem to work. When I plugged in the distance it made the issue worse. Then I tried using a small number and that seemed to make it better. So something like .09 was helping keep the numbers closer to 0 but it still was leaving issues.

Edit: I think I need to use the Arc Length Formula so when I get the chance I will try that.

Can you explain ow your doing this because I’m actually struggling with the same problem. When ever it turns my coordinates change as if the robot goes in a circle. based on the distance my tracking wheel has gone for the radius.

So I used the formula for calculating the circumference of a circle multiplied by the change in angle of the robot, and you subtract this from the wheels distance traveled. I wasn’t able to get it perfect because I couldn’t get the right distance to the center of the tracking wheel, which you use as the radius.

I found a fix for this but its shown in V5 blocks not sure if it will help but here it is:

That is correct. The arc length formula is this:

Arc length: distance traveled by the mid encoder
Radius: distance from the mid encoder to the center of rotation

With these values, you can calculate theta. Then, you can compare the arc length theta with the actual rotation of the chassis and figure out how much horizontal distance the robot actually traveled.

1 Like