Fixed AngleController * May want to make more efficient
I tested autonomous and added functionality to AutonomousDrive.  It can now intake and move at the same time.