Go Down

Topic: Odometry to Cartesian calculations - calculating bearing (Read 1 time) previous topic - next topic

RichMo

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: [Select]
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?

PaulS

Quote
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.

RichMo

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.

PaulS

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

That depends.

Quote
(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.

RichMo

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: [Select]
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


Hi RichMo,
Am working on a mobile robot and using rotary encoder and arduino mega. I have problem in determing the position of the robot. Your code catches my interest but i got confused when you talked of pulses per meter in your definitions. And moreso, i dont understand these 3 lines
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
I must admit that i am quite new in this field. Your help will be of immense help to me
Thanks in advance
Nelson

solderspot


I must admit that i am quite new in this field. Your help will be of immense help to me
Thanks in advance
Nelson


Hi Nelson,

You should  read this: http://rossum.sourceforge.net/papers/DiffSteer/

It explains the process quite well.
solderspot
journal: http://solderspot.wordpress.com
github: https://github.com/solderspot

Go Up