Cleaned up my auton code
Testing with my piston
added blue and red auton