Fixed bugs in autonomous