[Tom Bottiglieri]

Originally Posted by Goldeye

My confusion is: How do you calculate distance_traveled_this_cycle if the wheels are turning at different rates?

Every time the example navigation program loops, it is creating a vector in component form to express the position of the robot in relative to its position the in the previous loop of the program. Therefore, your position on the field at any particular point in time is the summation of all the relative position vectors created up until that particular point in time. Most teams just keep a running total of the X and Y components and throw away the vector data. Keep in mind, it is perfectly possible to keep track of all of those relative position vectors, then create a graph using a parametric equation (with X and Y as parameters of Time) to study later on how the robot moved during the match.

Back to the question at hand... To find the magnitude of distance traveled simply average the left and right side encoder counts. This holds true because this code is really tracking the motion of the center of mass of the robot, not the entire robot. As an example, think of 2 people running side by side, each holding the end of a 10 ft long stick. In the middle of the stick is a big red dot. When the 2 people run at the same speed, the red spot stays directly in between them. When one starts to slow down, the red dot still stay exactly in between the 2 men, but the angle the stick is at has now changed. The red dot signifies the Center of the robot, and how its motion is affected by linear forces on either side of it.

Here is an example of basic position calculator.

//Using Accelerometer....

//position is the total linear distance moved since start up
velocity += GetAcceleration();
position += velocity;

//Using Encoders...


//position is the total linear distance moved since start up

position = ( GetLeftEncoderCount() - GetRightEncoderCount() ) / 2 ;

Now throw what your total travelled distance is into this:

distance=position-distance; //find distance travelled since last loop
heading = GetGyroAngle(); //get instantaneous heading


//accumulate X components of relative position vectors
X_coord += distance * cos(heading); 

//accumulate Y components of relative position vectors
Y_coord += distance * sin(heading);

Just make sure something like that loops very fast. (The IFI default code loop should be fast enough)

So, I've told you how to find where you are. The balls in your court to find where you want to be and how to get there.

Good Luck!

EDIT: If none of this is making sense to you, check this out!