Modified motor_control and calibrate