Función para el control de un servomotor HS322

Para el uso de la función solo restaría hacer la declaración de la misma en los encabezados del código, seria de la siguiente forma:

VOID SERVO(INT16 U, INT16 D);
VOID LONG_DELAY_US(LONG C);

La variable U es el ángulo de apertura t la variable D es el tiempo del pulso en cero se puede usar 20000, pero depende de la velocidad que se desee para el servo.

// FUNCION DE CONTROL DE SERVO
VOID SERVO(INT16 U, INT16 D){
INT I; // REPETICIONES DE LOS PULSOS
INT16 T; // VQARIABLE DE TIEMPO
T=2400-(U*10); // CALCULA EL TIEMPO DEL PULSO
FOR(I=0;I<30;I++){
OUTPUT_D(0X01); // ACTIVA LA SALIDA D
LONG_DELAY_US(T);
OUTPUT_D(0X00); // DESACTIVA LA SALIDA D
LONG_DELAY_US(D); // DELAY DE APAGADO
} // FOR
} // FUNCION SERVO

-- JUNTAR CON DELAY US

// FUNCION DE DELAY EN US
VOID LONG_DELAY_US(LONG C){
CHAR i; // VARIABLE DE CONDICION
i=(CHAR)(C>>8); // IGUALA I A C EN 8 BITS
WHILE(I >= 1) { // CONDICION DE TIEMPO MINIMO
I--; // LA VARIABLE VA DISMINUYENDO
DELAY_US(253); // DELAY DE 253 US
RESTART_WDT(); // RESETEA EL CONTADOR DEL WATCHDOG
} // WHILE
DELAY_US((CHAR)C);
} //FUNCION DELAY US