added more start state logic & small bug fixes

This is working code: drove to 4 waypoints successfully