MOTOR_CONTROL: Replace Torque controller with PID controller