Go Down

### Topic: Odometry to Cartesian calculations - calculating bearing (Read 2078 times)previous topic - next topic

#### RichMo

##### Jan 04, 2013, 11:33 pm
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

#1
##### Jan 04, 2013, 11:45 pm
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

#2
##### Jan 05, 2013, 12:05 am
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

#3
##### Jan 05, 2013, 12:11 am
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

#4
##### Jan 05, 2013, 09:46 pm
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 `

#### Nelsonvuga

#5
##### Mar 18, 2014, 10:51 pm
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
Nelson

#### solderspot

#6
##### Mar 19, 2014, 12:06 am

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

Hi Nelson,

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

Go Up

Please enter a valid email to subscribe