I've developed and debugged code for autonomous GPS navigation, but I'm not sure that sharing the code will do you much good. To get some ideas on developing code you may want to consider the following:
-
Given GPS coordinates for A and B where A is your starting position and B is your desired position
-
Calculate direction from A to B
Then repeat the following in a loop:
- Get current position from GPS
- Calculate direction from previous GPS position to current (this is your direction of travel)
- Calculate the angle difference between desired course line and current course
- Use the angle difference to determine if you need to turn vehicle left or right
- Then drive on and repeat above steps until you reach target
There is of course much more to this to get smooth steering control (using some sort of PID algorithm, adding a Z-gyro and calculating distance from initial course line will help a lot), but I think you're better off getting the basics down first. See your vehicle deviate from the course line and actually compensate to move towards the course line is sign of good progress.
The math for doing above is somewhat involved and an additional challenge is only having floats (less precision) to work with. A good source for formulaes you need can be found here: