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);
currentGlobalThetaDegrees = currentGlobalThetaRadians * 180.0 / pi;
float deltaThetaRadians = currentGlobalThetaRadians - previousGlobalThetaRadians;
if (deltaThetaRadians == 0.0){
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 avgTheta = previousGlobalThetaRadians + deltaThetaRadians / 2.0;
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;
previousGlobalThetaRadians = currentGlobalThetaRadians;
list<float> pos {currentGlobalX, currentGlobalY, currentGlobalThetaDegrees};
return pos;
}
This all runs continuously in a loop inside its own task.