Robot velocity controller completed. Waypoint navigation written but not tested.