The AGV code has been improved tons with lots of help from Bill porter, Thanks Bill. A lot of cleaning up and basic improvements were done but so were major ones. Added PD control instead of the plain P like before. The code runs faster at 10hz. The best part by far is the added functionality of storing waypoints in EEPROM and using the Ardupilot config tool to upload the points. Just today i ended up plugging the ESC into the on board receiver that the truck came with so i can control the speed manually (seen in the video above. Sorry for the shaky beginning).
As always the full story can be found here