Working shift code, reliable auton. fixed some bugs.