PLEASE READ THE ADDED COMMENTS

I made modifications to the methods to better suit the autonomous
methodology read the comments email me if its confusing
Jacobkirlanstout@gmail.com