I didn't do an in-depth review, but I'd recommend breaking your functions up a little more so they aren't super-long. It also makes verifying the logic a lot easier.
Also, I noted some Timer.delay() bits in there... keep in mind that if you delay, then nothing else can happen on your robot during that time (including feeding the watchdog, and driving), so you'll end up with the robot freezing when doing certain things.