Added calibrate_ESC function to main (disabled)