next topic »

[on:]

Hi,

I have a robot using differential drive and with a quadrature encoder on each motor. I'm trying to plot its position on a screen based on the odometry data. I have this example of calculation, but am having difficulty with the calculation for bearing.

http://www.seattlerobotics.org/encoder/200610/article3/IMU%20Odometry,%20by%20David%20Anderson.htm

I've simplified the example for the above link in the code below.

Code:

theta = 0;
WHEEL_BASE = 20;

void loop {
                L_Dis = GetOdometer(L);  // Get odometer reading (distance since last sample)
                R_Dis = GetOdometer(R);  // Get odometer reading (distance since last sample)

                distance = (L_Dis + R_Dis ) / 2; // Calculate total distance travelled

                theta += (L_Dis - R_Dis ) / WHEEL_BASE; // Accumulate total rotation
                theta -= (float)((int)(theta/TWOPI))*TWOPI; //Clip the rotation to plus or minus 360 degrees

/* now calculate and accumulate our position in inches */
                Y_pos += distance * cos(theta);
                X_pos += distance * sin(theta);
 }

To me it looks like theta would just become zero every time.  the line commented "Clip the rotation to plus or minus 360 degrees" seems to basically subtract theta from theta.

Does anyone have any suggestions or similar examples for converting odometry data to a Cartesian coordinate?

[Reply #1 on:]

To me it looks like theta would just become zero every time.  the line commented "Clip the rotation to plus or minus 360 degrees" seems to basically subtract theta from theta.

No. It is subtracting 360 degrees or 0 degrees from a value. Suppose that theta was 150 degrees.

Then, theta/360 would be 0. 0 times 360 is 0. 150 - 0 = 150.

Suppose, instead that theta was 370 degrees. 370/360 = 1. 1 * 360 = 360. 370 - 360 = 10.

[Reply #2 on:]

if theta = 100, then isn't (100/TWOPI)*TWOPI just 100 again?  (TWOPI is 6.284....)

When I use this code, theta always comes out as 0.
As you probably guessed I suck at maths. I don't see where you got 360 from.

[Reply #3 on:]

if theta = 100, then isn't (100/TWOPI)*TWOPI just 100 again?

That depends.

(TWOPI is 6.284....)

Is it? Or is it 360?

If it IS 6.284, then (100/TWOPI)*TWOPI would be 100, assuming that theta is 100 and is an int.

If it is 360, then (100/TWOPI)*TWOPI would be 0, assuming that theta is 100 and is an int.

This is because 100/360 is 0, not 0.2777.

[Reply #4 on:]

Thank you, it was simply my improper use of variable types that was giving me the problems. For reference here is the code I used for making it work..
      

Code:

double PI = 3.14159265359;
double TWOPI = PI * 2;
double ticksPerM = 105;  // Odometer pulses per meter
double wheelBase = 0.35; // Wheelbase in meters
double pixelsPerMeter = 10; // Multiplier for how many pixels represent 1m

double distance = ((m_odometerVals[0]/ticksPerM) + (m_odometerVals[1]/ticksPerM)) / 2; // Calculate distance moved in meters
theta += ((((m_odometerVals[0]/ticksPerM) - (m_odometerVals[1]/ticksPerM)) / wheelBase)); // Accumulate change in bearing;
theta -= ((double)((int)(theta/TWOPI))*TWOPI); // clip to +/- 360
xPos += pixelsPerMeter * distance * sin(theta);
yPos += pixelsPerMeter * distance * cos(theta);

PlotPoint((int)xPos,(int)yPos); // Plot the pixel on the screen