Odometry to Cartesian calculations - calculating bearing

Using Arduino Project Guidance
1

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.

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?

2

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.

3

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.

4

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.

5

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

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
6

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

7

Nelsonvuga:
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: A Tutorial and Elementary Trajectory Model for The Differential Steering System

It explains the process quite well.