GPS Sensor, how to code?

Hello, I was curios how I would code a GPS Sensor in text code to go to a specific point. Specifically would make it way better for our bot.

Check my post history for lots of good stuff. The GPS is a set of eyes, but we need to code both the brain and the feet.

Now there may be libraries in some of the Non-VEX IDEs/Languages you can use, but I will attempt to help you program your very own.

Eyes: The GPS sensor will tell you where you are and which way you are looking. If you want to go somewhere you will need a brain and feet to figure out where to go and how to tell your motors to get there.

Brain: You will need to use trig to determine how far in front of you your target is, and how far to the side (left or right). You will need to determine some forward value by using inverse tan of your target coordinates, and then subtracting your own heading so you can tell what angle you need to turn to. Output this angle as our desired turn value.

Then you will need to know whether to drive forward or back. For this you will basically do the same thing but 90 degrees offset. If you were to translate horizontally towards it (lets say to your right) then there is no need to translate forward or back at the moment. However if the difference between the angle to the desired location and 90 degrees to the right of dead center of your robot, you will need to start turning.

Therefore we will use the targetHeading to be the angle we need to turn towards, and targetDrive to be the forward or back amount.

Feet: A Turn and Drive PID, with targetDrive being the translational error term, and targetHeading be the rotational error term. Then just add in the derivative and integral terms and tune it up.

left.spin(forward, driveOut+turnOut, volts); 
right.spin(roward, drive-turn, volts);

Then have your self a good time. This is of course not including things like path finding, feed forward, pivot adjustments, or destinationheading. Just gets you to the destination with as little processing as you can muster.