Added a few more logging statements and sped up the robot. GPS navigation is now working