Odometry Turning X & Y Gain

While making an odometry system based on 5225 Pilons’ document we came across a problem. While turning the robot the x and y coordinates start to gain and then after ~180 they start to recede, as if the robot is traveling in a circle. In our video we turn the robot clockwise but the x and y values drift up to 7 inches off. We are using 3 tracking wheels and have read other topics such as Odometry Turning Help and Odometry Turning Issue but they didn’t solve our issue.

Our code (PROS):

``````list<float> OdomPosTracking(float leftWheelOffset, float leftWheelCir,
float rightWheelOffset, float rightWheelCir,
float centerWheelOffset, float centerWheelCir,
float xInit, float yInit, float tInit)
{
float currentLeftPositionCentidegrees = leftTracking.get_position();
float currentRightPositionCentidegrees = rightTracking.get_position();
float currentCenterPositionCentidegrees = centerTracking.get_position();

float currentLeftPositionInches = currentLeftPositionCentidegrees / 100.0 / 360.0 * leftWheelCir;
float currentRightPositionInches = currentRightPositionCentidegrees / 100.0 / 360.0 * rightWheelCir;
float currentCenterPositionInches = currentCenterPositionCentidegrees / 100.0 / 360.0 * centerWheelCir;

float leftChangeInches = currentLeftPositionInches - previousLeftPositionInches;
float rightChangeInches = currentRightPositionInches - previousRightPositionInches;
float centerChangeInches = currentCenterPositionInches - previousCenterPositionInches;

previousLeftPositionInches = currentLeftPositionInches;
previousRightPositionInches = currentRightPositionInches;
previousCenterPositionInches = currentCenterPositionInches;

float currentGlobalThetaRadians = startingThetaRadians + (currentLeftPositionInches - currentRightPositionInches) / (leftWheelOffset + rightWheelOffset);

localChangeX = centerChangeInches;
localChangeY = rightChangeInches;
}
else{
localChangeX = 2.0 * sin(deltaThetaRadians / 2.0) * (centerChangeInches / deltaThetaRadians + centerWheelOffset);
localChangeY = 2.0 * sin(deltaThetaRadians / 2.0) * (rightChangeInches / deltaThetaRadians + rightWheelOffset);
}

float localChangeTheta = atan2(localChangeX, localChangeY);
float localChangeR = sqrt(pow(localChangeX, 2.0) + pow(localChangeY, 2.0));

float modLocalTheta = localChangeTheta - avgTheta;

float globalChangeX = localChangeR * cos(modLocalTheta);
float globalChangeY = localChangeR * sin(modLocalTheta);

currentGlobalX += globalChangeX;
currentGlobalY += globalChangeY;

return pos;
}
``````

This all runs continuously in a loop inside its own task.

Most likely your right/back wheel offset is wrong, or the magnitude is right but it needs to be reversed.

Also use an imu for heading.

3 Likes

Our offsets are as close as we can measure (within 0.1 inches). We have tried using an inertial but it did not solve our issue.

Did you at least try reversing them?

1 Like