Implementing Odometry into my code

I currently have a working odometry code on an X drive chassis. When I start the code, I can display the values for X, Y, and heading on the brain and these values remain quite accurate when I move the robot around. However, I’m not sure how I should implement my working code into a driving function. I assume that I should make a drive to point function with parameters of (x, y, heading) but I don’t know how to get the robot to control the speed of the individual wheels so that it makes a straight line from point A to point B.

I actually have the same problem. What you need to do (probably) is make a pure pursuit code. From what I know it is very hard to make, and you pretty much need to be a mathematical genius to understand the maths involved (OK I know someone is gonna correct me here but some of us struggle with maths). Oh and you can’t find any example codes online, and no-one will post them, because it is always best to learn something yourself (I actually agree with this).
About Pure Pursuit: From my understanding, you pretty much set points that the robot will aim for (called waypoints), and then the algorithm adds more points in between and smooths the path (don’t ask me how), and all of this leads to approximately the most accurate way to move your robot around (yes I know someone will correct me here). Anyways, I am doing a bunch of research into how to do it, but haven’t come up with anything that I completely understand. I’m sure someone who understands better than me can post some documents that explain a bit better. I know there are some good ones out there.

Here’s a good place to start learning pure pursuit
pure_pursuit-1.pdf (674.3 KB)
On the simpler end, you can PID turn to a point using atan2 and drive until you reach the point using a second PID. You could also try combining them into one loop with a double PID where the speed is controlled by distance and the the difference in speed is controlled by the angular error.

A simpler explanation of pure pursuit is that the robot trys to follow a carrot point. This carrot point is found by drawing a circle with a set radius around the robot and finding where the circle intersects the path.


So much of coding has already been done. Many times, you will find someone else has already written a library that does what you want to do. There’s still time and effort involved in learning how to use it (and, whether you trust the author to have written the code correctly).

Search the forum; search the google machine. It took me 5 seconds to find this topic

This is a fantastic first-pass take of Pure Pursuit, at least the “path generation” portion. The second part is how the algorithm uses what it knows about the robot’s current position and finds the closest point within a specific distance on the generated path to drive towards.

The math involved really only assumes knowledge of trig, at least for the following portion. The path-generation part can get a bit intimidating when talk about Bézier curves crops up. But Pure Pursuit can work reasonably well even with simple path-generations that simply connects each way-point with a straight line and simple linear interpolation for “smoothing” the path.