Includes many debug statements on autonomous
Fixed bugs. I think it works!!!