I now have a compass module and i wrote some better code. The compass module is just a life saver. It is really accurate and just makes programming easier. You use i2c to communicate with it and just a snippet of code. Just make sure you mount it level and away from the electronics.
My new code has improved steering. I guess its proportional but i do not know if it is definition proportional drive. I came up with this little formula of delay=degree error / 180 *3000. It takes about 3 secs to turn 180 degrees so 3000 was my number i could tweak. Of course it is out of 180 because that is the most i will be turning either left or right. Our degree of error is how much we are off from our current heading to the heading we need to be at. Simply subtract the current heading from the desired heading and take the absolute value and presto! This code seems to work really well. I tested this in a parking lot plus i had 2 waypoints this time. (see video). You can see the results. the waypoints were given a radius of 2 meters but it normally gets closer than that. I will reduce the radius next time. The AGV is programmed to stop when it reaches its last GPS point. I ordered a l298 motor driver and will update this post when i have that on the AGV and i will have a more complicated path of waypoints.