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);
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.
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);
}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.
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);
}
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!