# Odometry Values Not Changing

When my robot moves the odometry X and Y position do not change. All the the other values change and if I directly add the encoder values the X and Y position change. I am wondering if my equation is incorrect or the values are to small when the equation runs?

``````LocalRobotPosX = (2 * sin(ChangeAngle / 2)) * (WheelTravelX2 / ChangeAngle);
LocalRobotPosY = (2 * sin(ChangeAngle / 2)) * (WheelTravelY2 / ChangeAngle);
``````
1 Like

The sine function outputs in radians, so you’re probably getting a small number. Try multiplying each line by 180 /π if you need the position in degrees.

2 Likes

Thank you. I will try that tomorrow.

You’ll still want the radians to degrees conversion, but the main reason for the position not changing is that you’re using ‘Position =’ instead of ‘Position +=’ (this adds the equation to itself repeatedly).

``````while(1) {

PositionX += cos(Angle * 3.1415926536 / 180) * (Tracker.position(deg) - LastPosition);
PositionY += sin(Angle * 3.1415926536 / 180) * (Tracker.position(deg) - LastPosition);
LastPosition = Tracker.position(deg);
Angle = Gyro.rotation(deg);
}``````
5 Likes

Ya that helped, but I am still having an issue that my position only goes up no matter where the robot moves. I beileve it is because of my Cartesian to polar to Cartesian coordinate conversion.

``````float RobotPositionX;
float PreviousRobotPositionX;
float RobotPositionY;
float PreviousRobotPositionY;
float RobotAngle;
float PreviousRobotAngle;

float LocalRobotPosX;
float LocalRobotPosY;
float LocalRobotPosX2;
float LocalRobotPosY2;
float PolarR;

float const OdomXOff = 0;
float const OdomYOff = 0;

float const WheelSizeIn = 2.75;

int Odom_Check() {
float WheelTravelX = 0;
float WheelTravelY = 0;
float WheelTravelX2 = 0;
float WheelTravelY2 = 0;

float PreWheelTravelX = 0;
float PreWheelTravelY = 0;

float AverageAngle = 0;
float ChangeAngle = 0;

OdomRotationX.resetPosition();
OdomRotationY.resetPosition();

while (!false) {
WheelTravelX = (((OdomRotationX.position(rotationUnits::rev)*360) * WheelSizeIn * M_PI)/360);
WheelTravelY = (((OdomRotationY.position(rotationUnits::rev)*360) * WheelSizeIn * M_PI)/360);
WheelTravelX2 = WheelTravelX - PreWheelTravelX;
WheelTravelY2 = WheelTravelY - PreWheelTravelY;
PreWheelTravelX = WheelTravelX;
PreWheelTravelY = WheelTravelY;
RobotAngle = Inertial.yaw();
ChangeAngle = RobotAngle - PreviousRobotAngle;
AverageAngle = PreviousRobotAngle + (ChangeAngle / 2);

if (ChangeAngle < .3 && ChangeAngle > -.3) {
LocalRobotPosX = WheelTravelX2;
LocalRobotPosY = WheelTravelY2;
}

else {
LocalRobotPosX = (2 * sin(ChangeAngle / 2)) * (WheelTravelX2 / ChangeAngle);
LocalRobotPosY = (2 * sin(ChangeAngle / 2)) * (WheelTravelY2 / ChangeAngle);
}

PolarR = hypotf(LocalRobotPosX, LocalRobotPosY);

LocalRobotPosX2 = PolarR * cosf(-AverageAngle);
LocalRobotPosY2 = PolarR * sinf(-AverageAngle);

RobotPositionX = PreviousRobotPositionX + LocalRobotPosX2;
RobotPositionY = PreviousRobotPositionY + LocalRobotPosY2;

PreviousRobotPositionX = RobotPositionX;
PreviousRobotPositionY = RobotPositionY;
PreviousRobotAngle = RobotAngle;
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print(RobotPositionX);
Brain.Screen.newLine();
Brain.Screen.print(RobotPositionY);
Brain.Screen.newLine();
Brain.Screen.print(OdomRotationX.position(degrees));
Brain.Screen.newLine();
Brain.Screen.print(OdomRotationY.position(degrees));
Brain.Screen.newLine();
Brain.Screen.print(RobotAngle);
Brain.Screen.newLine();
Brain.Screen.print(AverageAngle);
wait(10, msec);
}
return 1;
}
``````

Both X and Y position only go up?

``````    else {
LocalRobotPosX = (2 * sin(ChangeAngle / 2)) * (WheelTravelX2 / ChangeAngle);
LocalRobotPosY = (2 * sin(ChangeAngle / 2)) * (WheelTravelY2 / ChangeAngle);
}
``````

Looking at this, I’m not certain how this is calculating X and Y position. ‘ChangeAngle’ is updating once every 10 milliseconds (pretty quickly), so it will be fairly small.

And looking at the ‘if’ statement that comes before this:

``````if (ChangeAngle < .3 && ChangeAngle > -.3) {
LocalRobotPosX = WheelTravelX2;
LocalRobotPosY = WheelTravelY2;
}
``````

The loop might run this section instead of the second part since ‘ChangeAngle’ is small, regardless of the robot’s angle. So your X position and Y position would just equal the wheel rotation, not field position.

The main idea when calculating coordinates is increasing/decreasing the impact each wheel has on X and Y position by using sin/cos. You want to multiply the change in wheel rotation by the sin/cos of the robot’s current angle. The wheel rotation is ‘how far I’ve gone’ and the sin(Angle) or cos(Angle) is ‘how much that distance affects X or Y position.’

If you’re driving forwards at 0°, only Y position should increase.

You find the change in rotation…
Rotation - LastRotation = RotationChange

…and find how much to multiply that value by
cos(0 • π/180) = sin(0) = 1

The Y position gained from this would be Y = 1 * RotationChange, as this is a straight line.

When the angle is different, 45° for example, Y position would still increase, but at a lesser amount.

Y = cos(45 • π/180) • RotationChange, Y = 0.707 • RotationChange

As the Angle increases, cos(Angle) decreases. If the robot is facing 90° (moving along the X-axis), cos(90) = 0, meaning that RotationChange has no effect on the Y position.

This is how you calculate Y position. To calculate X position, simply swap the cos(Angle) to sin(Angle). sin(90) = 1, So when moving along the X-axis, X position will increase.

PositionY = cos(Angle • π/180) • RotationChange
PositionX = sin(Angle • π/180) • RotationChange

``````PositionY += cos(Angle * 3.1415926536 / 180) * (Tracker.position(deg) - LastPosition);
PositionX += sin(Angle * 3.1415926536 / 180) * (Tracker.position(deg) - LastPosition);
``````

Coordinate calculation for two tracking wheels would look something like this:

``````while(1) {

PositionX += cos(Angle * 3.1415926536 / 180) * (TrackerX.position(deg) - LastPositionX) - sin(Angle * 3.1415926536 / 180) * (TrackerY.position(deg) - LastPositionY);
PositionY += sin(Angle * 3.1415926536 / 180) * (TrackerX.position(deg) - LastPositionX) - cos(Angle * 3.1415926536 / 180) * (TrackerY.position(deg) - LastPositionY);
LastPositionX = TrackerX.position(deg);
LastPositionY = TrackerY.position(deg);
Angle = Gyro.rotation(deg);
}
``````

This is our code (we just added a second tracking wheel today, but the concept remains the same). Make sure to use ‘PositionX/Y +=’ instead of ‘=’ so it adds up each little increment in position, otherwise it will give 0.

That concludes this TED talk. Hope this helps.

8 Likes

Thank you. I will try this today and report back how it goes. I do have one question. If the angle is 0 then wont the position not change even if the robot moved?

Wait sorry never mind I understand. If the angle is 0 then only the Y can change.

We literally started odometry this week, and I wanted to point out that the last piece of code I sent doesn’t work 100%. Something weird with the Y value only decreasing while facing 90° or 270°.

Here’s what we got working today after some trial and error:

``````while(1) {

PositionX += sin(Angle * M_PI / 180) * (TrackerY.position(deg) - LastPositionY) + sin((90 + Angle) * M_PI / 180) * (TrackerX.position(deg) - LastPositionX);
PositionY += cos(Angle * M_PI / 180) * (TrackerY.position(deg) - LastPositionY) + cos((90 + Angle) * M_PI / 180) * (TrackerX.position(deg) - LastPositionX);
LastPositionX = TrackerX.position(deg);
LastPositionY = TrackerY.position(deg);
Angle = Gyro.rotation(deg);
Pause(20);
}
``````
3 Likes

Ya I was having a similar issue but I set my angle to be the absolute value of the inertial rotation. That fixed my issue. I also will test it tomorrow but subtracting your (TrackerY.position(deg) - LastPositionY) by the sin or cos multiplied by the tracking wheel offset, whether x or y, should fix any issues that arise from the tacking wheels not being centered.

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

LocalRobotPosX = sin(RobotAngle) * (WheelTravelX2) - cos(RobotAngle) * (WheelTravelY2);
LocalRobotPosY = cos(RobotAngle) * (WheelTravelX2) - sin(RobotAngle) * (WheelTravelY2);

RobotPositionX += LocalRobotPosX;
RobotPositionY += LocalRobotPosY;

PreWheelTravelX = WheelTravelX;
PreWheelTravelY = WheelTravelY;
``````

I also have my code math fragmented more so I can see and change it better.

Is this the whole code for position tracking with an inertial sensor? I came up with something similar but I did not have a robot on me and I did not think that this would be enough. Does this actually work?

That’s most of it. You’ll need more than just the inertial sensor since you also need a 1-2 tracking wheels to get distance.

I was referring to an inertial sensor + 2 wheels instead of 3 wheel odometry, but thanks!