Started to work on servo_calibration.
Started to work on servo_calibration. Everyting works fine.
Still need to set the parameters of servo12c automatically.