Add ESC calibration routine