problema con servomotore e libreria grbl

Salve.
Ho recentemente costruito un piccolo incisore laser con arduino uno, firmware grbl e software Laser grbl.
Il tutto funziona bene.

Ora voglio sostituire il modulo laser con un servo che comanda una penna.

Il servo usato è MG996R (duty cicle 20ms 50hz 5V) collegato al pin 11.
Firmware grbl servo (GitHub - robottini/grbl-servo: grbl 0.9i with Servo motor support) modificato nei parametri:

definire RC_SERVO_SHORT 15 // Timer tick per durata dell’impulso di 0,6 ms (9 per 0,6 ms)
definire RC_SERVO_LONG 32 // Timer tick per durata dell’impulso 2,5 ms (39 per 2,5 ms)

con il valore rispettivamente di 9 e 39 come indicato in gitHub per muovere il servo di 180°.

Ho provato ad inviare immagini con laser grbl ; i motori step fanno il loro lavoro ma il servo non si muove.

Allora ho provato altri sw (universal g-code e altri sw cam) inviando le immagini in g-code.
Il servo si muove di pochi gradi in unica direzione e in modo incoerente.

Se qualcuno mi aiuta ringrazio.
Ciao