Pages: [1]   Go Down
Author Topic: Odometry to Cartesian calculations - calculating bearing  (Read 988 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 1
Posts: 17
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 653
Posts: 50881
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Newbie
*
Karma: 1
Posts: 17
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 653
Posts: 50881
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Newbie
*
Karma: 1
Posts: 17
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 2
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged

Portland, Oregon.
Offline Offline
Newbie
*
Karma: 2
Posts: 34
Hobbyist
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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


Pages: [1]   Go Up
Jump to: