Removed println`s
Put gearshift out of testmode in teleop and autonomous