Does anyone have any insight on 2 tracking wheels + IMU for odometry? I have seen 5225’s document and noticed how its using 3 tracking wheels for its equations. Also, Team_23880E’s skills code. I noticed how they have 3 tracking wheels namely because of their strafing ability, but they also incorporate the inertial sensor.
3 Likes
I use that for my team’s robots. You only need two tracking wheels perpendicular to each other when you use imu for heading since the two parallel tracking wheels in the three tracking wheel setup are for heading calculations. See the post below:
The drive type does not matter, you can still use two tracking wheels + imu even if you have an x drive. I sometimes use three tracking wheels just so that I can average the readings from the two parallel tracking wheels and place the third one at the center of the robot.
6 Likes
Thank you for your advice! However, I am still confused on this code. This team 23880E uses the inertial to get the orientation of the robot, but they also use the third wheel in their code. Since I am not using it, I need to remove the parts where its mentioning deltaDistS or anything relating to the third tracking wheel. But what do i replace it with?
//If we didn't turn, then we only translated
if(deltaTheta == 0) {
deltaXLocal = deltaDistS;
// could be either L or R, since if deltaTheta == 0 we assume they're =
deltaYLocal = deltaDistL;
}
//Else, caluclate the new local position
else {
//Calculate the changes in the X and Y values (INCHES)
//General equation is:
//Distance = 2 * Radius * sin(deltaTheta / 2)
deltaXLocal = 2 * sin(deltaTheta / 2.0) * ((deltaDistS / deltaTheta) + STrackRadius);
deltaYLocal = 2 * sin(deltaTheta / 2.0) * ((deltaDistR / deltaTheta) - RTrackRadius);
}
//The average angle of the robot during it's arc (RADIANS)
avgThetaForArc = currentAbsoluteOrientation - (deltaTheta / 2);
deltaXGlobal = (deltaYLocal * cos(avgThetaForArc)) - (deltaXLocal * sin(avgThetaForArc));
deltaYGlobal = (deltaYLocal * sin(avgThetaForArc)) + (deltaXLocal * cos(avgThetaForArc));
Although they have it in there, I’m almost certain that they are not actually using it after reading their code. Even if there are parts of the code that use the third tracking wheel reading, those lines are either not doing anything or can be replaced by one of the other two tracking wheel readings.
The short answer is nothing. However, I would highly recommend you go through the math yourself, try to understand 23880E’s code, and write your own. Directly copying other people’s code won’t help you learn.
8 Likes
I been looking at their document and a couple of forum threads and videos. This is the code I have come up with which is a mix of 23880E’s code (they pull directly from the document), and fixed it to only have the 2 tracking wheels. I haven’t tested this yet.
LPos = LTrack.position(rotationUnits::deg);
RPos = RTrack.position(rotationUnits::deg);
deltaDistL = ((LPos - LPrevPos) * M_PI / 180) * WHEEL_RADIUS;
deltaDistR = ((RPos - RPrevPos) * M_PI / 180) * WHEEL_RADIUS;
LPrevPos = LPos;
RPrevPos = RPos;
totalDeltaDistL += deltaDistL;
totalDeltaDistR += deltaDistR;
currentAbsoluteOrientation = (360 - Inertial9.heading(rotationUnits::deg)) * M_PI / 180.0;
deltaTheta = currentAbsoluteOrientation - previousTheta;
previousTheta = currentAbsoluteOrientation;
if(deltaTheta == 0) {
deltaXLocal = deltaDistR;
deltaYLocal = deltaDistL;
}
else {
deltaXLocal = 2 * sin(deltaTheta / 2.0) * ((deltaDistR / deltaTheta) + RTrackRadius);
deltaYLocal = 2 * sin(deltaTheta / 2.0) * ((deltaDistL / deltaTheta) - LTrackRadius);
}
avgThetaForArc = currentAbsoluteOrientation - (deltaTheta / 2);
deltaXGlobal = (deltaYLocal * cos(avgThetaForArc)) - (deltaXLocal * sin(avgThetaForArc));
deltaYGlobal = (deltaYLocal * sin(avgThetaForArc)) + (deltaXLocal * cos(avgThetaForArc));
while(currentAbsoluteOrientation >= 2 * M_PI) {
currentAbsoluteOrientation -= 2 * M_PI;
}
while(currentAbsoluteOrientation < 0) {
currentAbsoluteOrientation += 2 * M_PI;
}
xPosGlobal += deltaXGlobal;
yPosGlobal += deltaYGlobal;
task::sleep(10);
I use 3, but the axis with a single wheel is not centered, so using my method you could make a 2 wheeled version. I won’t post code, but the key I found is to figure out how many rotations one of the wheels makes for a given rotation of the robot (IMU). Using that, you can figure out how much of the tracking wheel’s rotation is due to robot rotation and not lateral movement. If you can subtract the lateral movement, you can then just use the rest of the wheels rotation with your normal trig to add into your X and Y values. Does that make sense?
1 Like
Hi, I’m the coder from 23880E and I wrote the code in question.
Originally, our robot had 3 tracking wheels (L, R, S). Later in the season we switched to using the IMU for heading because we found it more accurate. After we added the IMU, we still had all three tracking wheels in the drive, despite only needing to keep either L or R.
Please let me know if you have any other questions about implementing odometry!
4 Likes
Thank you so much, very surprised that the actual author would find this thread. One question -
//The starting x and y coordinates of the bot (INCHES)
//These distances are relative to some point (0,0) on the field
//Relative to: BOTTOM LEFT CORNER
double X_START = 56.5; //19.1
double Y_START = 8.5; //8.5
Can you explain why you chose the bottom left corner to be 0, 0? Did you measure the distances with a tape measure to the turning point of the robot?
The coordinate basis you use is really up to you. In my code, I set the bottom left corner of the field to be (0, 0) with the right being positive X and up being positive Y. I based my angles for headings off the unit circle. You can use whatever basis makes sense for you though!
And yes, I measured the distance from (0, 0) to the tracking center of the robot with a tape measure.
1 Like
Hey, as the beginner, I have a lot of question about the odometry. I have right now the chassis with 2 encoders (one vetical and one horizontal) and IMU. Everything seems fine than I got to converting the local values to the global. It will be really helpful if you will look up at my code and give me the reflection, how to fix it or how to make my code cleaner.
Thanks
#include "main.h"
using namespace pros;
// encoders values
int L_ENC_value = 0.0;
int M_ENC_value = 0.0;
// change in encoders
int lDelta = 0.0;
int mDelta = 0.0;
// previous encoders values
int last_lDelta = 0.0;
int last_mDelta = 0.0;
// distance in inches
float lDist = 0.0;
float mDist = 0.0;
// The current angle of the bot (RADIANS)
int currentAbsoluteOrientation = 0;
// The previous angle of the bot (RADIANS)
int last_Theta = 0;
// change in the angle(RADIANS)
float deltaTheta = 0;
// average theta for the arc
float avgThetaForArc = 0;
// local position
float x_pos_local = 0;
float y_pos_local = 0;
// the global postion in arc
float delta_x_pos_global = 0;
float delta_y_pos_global = 0;
// global position of the robot
float x_pos_global = 0;
float y_pos_global = 0;
int odomtrack()
{
while (true)
{
// getting values from encoders
L_ENC_value = L_Encoder.get_value();
M_ENC_value = M_Encoder.get_value();
// get the change of the values
lDelta = L_ENC_value - last_lDelta;
mDelta = M_ENC_value - last_mDelta;
// trasform values to inches
lDist = lDelta / degrees_side_per_inch;
mDist = mDelta / degrees_side_per_inch;
// calculate the current angle of the robot in radians
currentAbsoluteOrientation = (360 - (int)inertial.get_rotation()) * drive_pi / 180;
// get change in the angle
deltaTheta = currentAbsoluteOrientation - last_Theta;
// transform my current values to previous values
last_lDelta = L_ENC_value;
last_mDelta = M_ENC_value;
last_Theta = currentAbsoluteOrientation;
// if there is no angle change, then it is only change in encoders
if (deltaTheta == 0)
{
y_pos_local = lDist;
x_pos_local = mDist;
}
// calculate the x-position and y-position
else
{
y_pos_local = (lDist / deltaTheta + dRobotSide) * (2 * sinf(deltaTheta / 2.0));
x_pos_local = (mDist / deltaTheta + dRobotMiddle) * (2 * sinf(deltaTheta / 2.0));
}
// the average angle in the arc
avgThetaForArc = currentAbsoluteOrientation - (deltaTheta / 2);
// calculate the global change in the position
delta_y_pos_global = (y_pos_local * sinf(avgThetaForArc)) + (x_pos_local * cosf(avgThetaForArc));
delta_x_pos_global = (y_pos_local * cosf(avgThetaForArc)) - (x_pos_local * sinf(avgThetaForArc));
// uptade my global position of the robot
if (abs(lDelta) > 1 && abs(mDelta) > 1)
y_pos_global += delta_y_pos_global;
x_pos_global += delta_x_pos_global;
int Delta_deg = inertial.get_rotation();
// printing the values
lcd::print(3, "x:%f, y:%f, ang:%d", x_pos_global, y_pos_global, Delta_deg);
lcd::print(4, "last l: %d, last m: %d, last a:%d", last_lDelta, last_mDelta, deltaTheta);
lcd::print(5, "%f,%f", y_pos_local, x_pos_local );
delay(20);
}
return 1;
}
ps. I am using pros, it should be similar to vex code
Does the code work? I noticed you declared your rotation in radiants as an integer
Also my team is using 2 wheel + IMU for odometry we find it to be easier to implement and more accurate than 3 wheel
AHHHHHHH,
true-Thanks
Unfortunetly, it is still not working
Im in the process of converting our drive and odometry code over from vexcode pro to pros and im have trouble with the odometry part too
Lots of numerical overflow and crashes
Ameture software engineering is engaging to say the least