Pinguino: rutina para controlar 18 servos !!

En mi «artículo idea» sobre Control de Servos en Paralelo, ya explicaba cual era mi idea para controlar más servos de los que en un principio se pueden controlar con un PIC si secuenciamos la gestión de los pulsos de todos los servos que vamos a controlar. Por eso, en ese articulo hablaba del control de servos en «paralelo», que opino que podría permitir controlar más servos a la vez. Y así es.

Finalmente decidí embarcarme en la programación de la rutina en ensamblador PIC, y después de estar pegándome unos días, decidí cambiar el ensamblador por el SDCC de Pinguino. La razón principal, porque quería completarla para el proyecto que estoy haciendo para ARDE, el PartyBot. Obviamente, mucho más sencillo de hacer en C que en ensamblador. Aquí comparto con vosotros el código de la rutina que he creado, que he probado y que ya he visto que funciona perfectamente.  En principio, quiero que la rutina se convierta más tarde en una librería, así que aprovecho para escribir este artículo también en ingles….

so, english follows below. For english version click here…

Como explicaba en el articulo anterior, la idea es hacer que el PIC gestione todos los bits de los puertos seleccionados como servos, de manera que al principio de cada ciclo de 20ms (que necesitan los servos para ser refrescados), se pongan a nivel alto todos ellos a la vez.

Después, teniendo en una tabla ordenados de menor a mayor los tiempos a los que se tienen que poner a nivel bajo cada uno de ellos, el PIC los vaya poniendo.

Ese intervalo es desde que ha terminado el primer milisegundo, hasta que termina el segundo milisegundo. Durante ese milisegundo es en el cual una rutina tan solo espera al momento adecuado en el que cada servo tiene que ser atendido para ponerlo a nivel bajo. La imagen que ponía en el articulo anterior es muy ilustrativa y permite visualizar lo que digo:

Además de eso, obviamente tenemos que tener:

• Una tabla con todos los valores que queremos que estén puestos cada servo.
• Una tabla que guarde los mismos ordenados (de menor a mayor),… y además decir qué servos tienen que cambiar a ese tiempo determinado.

El ejemplo resultará muy claro:

Tenemos la siguiente tabla de entrada con indice de servo y valor de su posición, que se genera con la función SetServo(servo, valor) :

Indice/pinguino pin	Posición
0	125
1	55
2	214
3	125

Después de ordenar esta tabla de entrada, la tabla resultante sería:

Tiempo	Servos a poner a nivel bajo
55	1
125	0 y 3
214	2

Ahora la rutina que se encarga de ponerlos a nivel bajo durante el segundo milisegundo, solo tendrá que estar pendiente de cuando llega el momento siguiente indicado en la tabla y poner los servos asociados, a nivel bajo. Suficientemente simple para que no perdamos tiempo durante esa rutina y además sea lo suficientemente rápida para que los tiempos no se vean degradados.
Por comodidad y simplicidad, he utilizado valores para el posicionamiento de los servos que van desde 1 hasta 250. Esto quiere decir que ese segundo milisegundo dividido entre 250 nos da una resolución máxima de 4 microsegundos. Además, como en un byte podemos almacenar hasta valores de 0 a 255, tenemos el espacio adecuado en un 1 byte.
Dado que pinguino utiliza un xtal de 20 Mhz, que está configurado para funcionar con USB, y que por tanto necesita pasar a través del PLL para multiplicar la frecuencia, tenemos que el reloj de la CPU resultante es 48Mhz, lo cual nos da 12 MIPS ( = Fosc / 4 ). Esto quiere decir que cada milisegundo se ejecutarán 12500 instrucciones, o también, 12500 ticks producirá TMR1 (que es el que voy a utilizar para las temporizaciones). Por otro lado, en 4 microsegundos se ejecutan 50 instrucciones, cosa que también es interesante tener en cuenta.

Una vez explicado eso, simplemente hay que planificar como y cuando generaremos las interrupciones:

1. Generamos una primera interrupción para poner los pulsos de todos los pines activados como servos a nivel alto, y configuramos el TMR1 para generar una interrupción al cabo de 1 ms.
2. Cuando a pasado ese primer milisegundo, se genera una interrupción y entramos directamente en la rutina para poner los servos a nivel bajo (de acuerdo a sus temporizaciones cada uno). Esta rutina dura 1 ms exactamente. Durante ese tiempo no salimos de la rutina de interrupción (es el coste de hacerlo de este modo). Terminado ese milisegundo configuramos el TMR1 para que genere otra interrupción al cabo de 18 ms, y lo lanzamos. Pero antes de salir de la rutina de interrupción consultamos un flag que nos dice si algún servo ha sido modificado de valor ( mediante la función SetServo ), y por tanto se necesita que se re-ordene toda la tabla de temporizaciones. Si es así, aún consumiremos más tiempo dentro de la rutina de interrupción (aunque esto no afecta a TMR1 que ya están contando para generar los 18 ms), pero nos aseguramos de que la tabla esté lista para todas los ciclos siguientes. Cuando devolvemos el control al flujo principal, se seguirá ejecutando el programa que hayamos hecho, hasta que los 18 ms hayan pasado, se genere otra interrupción e iniciemos el ciclo en el punto 1.

Así de simple.

Para gestionar qué servos están activos, y para gestionar qué servos hay que poner  a nivel bajo en sus determinados tiempos, guardamos las máscaras de cada bit de cada puerto en registros, para luego con operaciones lógicas como AND (para ponerlos a nivel alto) o XOR (para ponerlos a nivel bajo), los podamos modificar individual mente sin afectar al resto de bits.

La rutina está comentada y por tanto creo que también es fácil de entender.

Ahora el objetivo es convertirla en librería y que forme parte de la suite de Pinguino.

Si a alguien se le ocurren ideas de mejora, son bienvenidas !!!!

Aquí está el código. Para copiarlo, utiliza los botones que aparecerán dentro de la caja del código (arriba-derecha) cuando pases el ratón. Tan solo añadir que fueron creadas, compiladas y probadas con pinguino beta 8 :

// ------------------------------------------------------------------------------
// RUTINA PARA CONTROL DE HASTA 18 SERVOS CON PINGUINO 18F2550
// ==============================================================================
// Versión: 1.0
// Autor:  Jesús Carmona Esteban (Sphinx para ARDE)
// Fecha:  28/7/2010
//
// Esta rutina es para la version de Pinguino 18F2550
// y permite controlar hasta 18 servos simultaneamente. Es decir, se pueden
// activar todos los pines disponibles como controladores de servos.
// NOTAS:
// -  El xtal es de 20 Mhz, y la salida del PLL da una frecuencia
//    de 48Mhz => 12 MIPS (Fosc/4).
// - Recursos utilizados:
//        * Timer TMR1
//        * Posiciones de memoria: 98 aprox.
// - En esta rutina, la resolución es de 1 a 250 para cada servo.
//   es decir, una resolución minima de 4 microsegundos.
// - La función para indicar la posición de cada servo es SetServo(servo,valor).
// - Los servos son gestionados en paralelo por el PIC. Esto quiere decir que
//   todos los pines activados como servos se pondrán a nivel alto al mismo
//   tiempo, y volverán a nivel bajo cada uno al momento que se haya indicado
//   mediante la función SetServo.
//
//
// -----------------------------------------------------------------------------

#define PIC18F2550

// Valores minimos y maximos permitidos para los servos.
#define SERVOMAX 250
#define SERVOMIN   1

//variables
uchar phase=0;
uchar needreordering=0;
uchar timingindex;
uchar timedivision=0;
uchar loopvar;
uchar timings[4][18];  // Matriz bidimensional para contener valor de los timings y las mascaras que indican a que servos corresponden cada timing.
uchar activatedservos[3]={0x00,0x00,0x00};  // Matriz para guardar las mascaras de los pines activados para servos. Por orden los puertos B,C y A.
// Definición de valores de indices para utilizar con las matrices.
#define MaskPort_B  0
#define MaskPort_C  1
#define MaskPort_A  2
#define timevalue   3

uchar servovalues[18]; // Tabla donde se recogen todos los valores de los servos activados.

//Tabla de mascaras de todos los pines de Pinguino 18F2550:
const uchar servomasks[18]={0X01,0x02,0x04,0x08,0x10,0x20,0x40,0x80,0x40,0x80,0x01,0x02,0x04,0x01,0x02,0x04,0x08,0x20};

//miscelanea
uchar valor=1;

void ServosPulseDown()
{
 timingindex = 0;

 for(timedivision=0;timedivision < 251;timedivision++){
 if (timings[timevalue][timingindex] == timedivision){
 PORTB = PORTB ^ timings[MaskPort_B][timingindex];   // XOR del puerto con la máscara. Pondra a nivel bajo los bits correspondientes.
 PORTC = PORTC ^ timings[MaskPort_C][timingindex];
 PORTA = PORTA ^ timings[MaskPort_A][timingindex];
 timingindex++;
 }
 // La siguiente pieza de codigo en ASM crea el retardo preciso para hacer que cada 'loop' del 'for' dure aprox 4 microsegundos.
 __asm
 movlw 6
 movwf _loopvar
 bucle:
 NOP
 decfsz _loopvar,1
 goto bucle
 __endasm;
 }
}

void ServosPulseUp()
{
// Esta función pone a nivel alto todos los pines que tengan activados/definidos un servo.
 PORTB = activatedservos[MaskPort_B] & 0xFF;
 PORTC = activatedservos[MaskPort_C] & 0xFF;
 PORTA = activatedservos[MaskPort_A] & 0xFF;
}

void SortServoTimings()
{
// Esta función lee la matriz servovalues y ordena los valores de tiempo de menor a menor
// en la matriz timmings. Junto a cada valor de tiempo encontrado, se asocian las mascaras
// de los servos que tienen que ponerse a nivel bajo en ese timing concreto.
// El tener la matriz ordenada, permite a la función ServoPulseDown leer poner a nivel bajo
// secuencialmente los servos.

 uchar s,t,totalservos,numservos;
 uchar mascaratotal[3]={0x00,0x00,0x00};

 // inicializamos la tabla:
 for(t=0;t<18;t++){
 timings[timevalue][t]=255;
 timings[MaskPort_B][t]=0x00;
 timings[MaskPort_C][t]=0x00;
 timings[MaskPort_A][t]=0x00;
 }

 totalservos=0;
 t=0;
 while(totalservos<18) {
 numservos=1;
 for(s=0;s<18;s++) { // dentro de este for, vamos a buscar el valor menor que exista
 // entre los servovalues, y lo almacenamos en timings.

 // Caso para los servos del puerto B:
 if (s<8){
 // el siguiente if comprueba si el servo/pin ha sido activado y si ha sido revisado ya.
 if (servomasks[s] & mascaratotal[MaskPort_B] & activatedservos[MaskPort_B]){
 }
 else if (servovalues[s] < timings[timevalue][t]){
 timings[timevalue][t]=servovalues[s];
 timings[MaskPort_B][t]=servomasks[s];
 timings[MaskPort_C][t]=0x00;
 timings[MaskPort_A][t]=0x00;
 numservos=1;
 }
 else if (servovalues[s] == timings[timevalue][t]){
 timings[MaskPort_B][t] |= servomasks[s];
 numservos++;
 }
 }
 // Caso para los servos del puerto A:
 else if (s>12){
 // el siguiente if comprueba si el servo/pin ha sido activado y si ha sido revisado ya.
 if (servomasks[s] & mascaratotal[MaskPort_A] & activatedservos[MaskPort_A]){
 }
 else if (servovalues[s] < timings[timevalue][t]){
 timings[timevalue][t]=servovalues[s];
 timings[MaskPort_B][t]=0x00;
 timings[MaskPort_C][t]=0x00;
 timings[MaskPort_A][t]=servomasks[s];
 numservos=1;
 }
 else if (servovalues[s] == timings[timevalue][t]){
 timings[MaskPort_A][t] |= servomasks[s];
 numservos++;
 }
 }
 // Caso para los servos del puerto C:
 else {
 // el siguiente if comprueba si el servo/pin ha sido activado y si ha sido revisado ya.
 if (servomasks[s] & mascaratotal[MaskPort_C] & activatedservos[MaskPort_C]){
 }
 else if (servovalues[s] < timings[timevalue][t]){
 timings[timevalue][t]=servovalues[s];
 timings[MaskPort_B][t]=0x00;
 timings[MaskPort_C][t]=servomasks[s];
 timings[MaskPort_A][t]=0x00;
 numservos=1;
 }
 else if (servovalues[s] == timings [timevalue][t]){
 timings[MaskPort_C][t] |= servomasks[s];
 numservos++;
 }
 }

 }
 mascaratotal[MaskPort_B] |= timings[MaskPort_B][t]; // vamos acumulando las máscaras
 mascaratotal[MaskPort_C] |= timings[MaskPort_C][t]; // de los servos que ya hayamos
 mascaratotal[MaskPort_A] |= timings[MaskPort_A][t]; // revisado.
 totalservos += numservos;
 t++;
 //una vez recorrida esta tabla, todos los timings de los servos van de menor a mayor.
 }
 needreordering=0;  // indicamos que la tabla está ordenada y que NO es necesario reordenarla.
}

void EnableTimerInterrupt()
{
 TMR1H=0xFF;
 TMR1L=0x00;
 // timer 1 prescaler 1 origen del oscilador es interno = 48Mhz >> 12 MIPS
 T1CON=0x01;
 // Habilitamos las interrupciones para el timer1 en el registro PIE1
 PIE1bits.TMR1IE=1;
 // Habilitamos las interrupciones de perifericos
 INTCONbits.PEIE=1;
 // Habilitamos las interrupciones globales.
 INTCONbits.GIE=1;
}

void SetServo(uchar servo, uchar value)
{
 if(servo>=18)        // Comprobamos si el num de servo es valido.
 return;

 if(servo<8){
 activatedservos[MaskPort_B] = activatedservos[MaskPort_B] | servomasks[servo];
 } else if (servo>12) {
 activatedservos[MaskPort_A] = activatedservos[MaskPort_A] | servomasks[servo];
 } else {
 activatedservos[MaskPort_C] = activatedservos[MaskPort_C] | servomasks[servo];
 }

 if(value<SERVOMIN)  //  1 = Pulso de 1000 useg de duración
 value=SERVOMIN;
 if(value>SERVOMAX) // 250 = Pulso de 2000 useg de duración
 value=SERVOMAX;
 servovalues[servo]=value;

 needreordering=1;  //Indicamos que la tabla es nesario reordenarla, por haber introducido un nuevo valor.
}

//Interrupción para manejar los servos.
void UserInterrupt()
{
 if (PIR1bits.TMR1IF) {
 PIR1bits.TMR1IF=0;
 T1CON=0x00;
 if (phase) {
 //caso antes 1er ms. Levantamos los pulsos de los servos.
 ServosPulseUp();
 // Cargamos en TMR1 53035d (tambien 0xFFFF - d'12500) Con esto tendremos un delay de 1 ms.
 TMR1H= 0xcf;
 TMR1L= 0xb2;
 // timer 1 activado y prescaler 1 origen del oscilador es interno = 48Mhz >> 12 MIPS
 T1CON=1;
 phase = 0;
 } else {
 //caso antes 2do ms:
 //lo siguiente se lleva 1 ms de ejecución:
 ServosPulseDown();
 // Ahora los 18 ms, donde lanzamos la interrupción de nuevo.
 // Cargamos en TMR1 9285d (que es: 0xFFFF - [(4,5 x 12500(duracion 1ms)) = 0x2445] => a 4,5 ms)
 // Con esto y con el preescaler x 4, tendremos un delay de 18 ms.
 TMR1H= 0x24;
 TMR1L= 0x45;
 if (needreordering)    // Si están desordenados los reordenamos y retornamos de la isr.
 SortServoTimings();
 // timer 1 activado y prescaler 1:4 origen del oscilador es interno = 48Mhz >> 12 MIPS
 T1CON= ( 1 | 2 << 4 ) ;
 phase = 1;
 }
 return;
 }
}

void setup()
{
unsigned char a;

//Inicializamos los pines como salidas.
for(a=0;a<18;a++) {
 pinMode(a,OUTPUT);
 servovalues[a]=255;
 }

//A modo de demo:
//Hacemos que todas las puertas sean salidas para controlar servos
SetServo(0,1);
SetServo(1,20);
SetServo(2,30);
SetServo(3,110);
SetServo(4,123);
SetServo(5,200);
SetServo(6,21);
SetServo(7,41);
SetServo(8,251);
SetServo(9,61);
SetServo(10,254);
SetServo(11,11);
SetServo(12,133);
SetServo(13,199);
SetServo(14,90);
SetServo(15,21);
SetServo(16,1);
SetServo(17,100);

SortServoTimings();     // Hacemos una primera ordenación antes de
EnableTimerInterrupt(); // ...habilitar las interrupciones.
}

void loop()
{
 // A modo de demo:
 // Con este loop los valores de los servos 0,2,10 y 17 variarán constantemente.
 delay(250);
 SetServo(0,valor);
 SetServo(2,255-valor);
 SetServo(10,valor);
 SetServo(17,255-valor);
 valor++;
}

(English version)

As I was explaining in my previous article, the idea is to make that PIC manage all bits of all ports set as servo drivers, in a way that at the beginning of every 20ms cicle(what is needed for driving a servo), be set up high all at the same time.

After that, having an array of timings already sorted out (from lesser to greater), PIC can use it to set servos low one by one when requiered.

That interval starts after first milisecond ends to second milisecond ends. Along that a routine will wait for the specific moment when every servo pulse must be set down. The following picture helps to envision what I have just explained :

Besides of that, obviously we need:

• An array with all servo position values.
• An array that stores those position values sorted out,… and that additionally includes the numbers of servos that must be set at that especific position values (or timings)..

The following example is quite illustrative:

We have an array with the following servo number and position values set:

Index	Position
0	125
1	55
2	214
3	125

After sorting out those values, a new array will be generated containing something like this:

timing aka position values	Servos to set up low level
55	1
125	0 y 3
214	2

Now the routine in charge of setting those to low level during the second milisecond, only will be ready looking to the time of TMR1 and the first time stored in this second table, to set the indicated servos to low. Simple enough to not waste time during the 1 ms routine, and let the routine to be fast enough to not degrade timings further that the milisecond.
For simplicity, I’ve used positing values from 1 to 250. This means than that milisecond divided by 250 produces a maximum resolution of 4 microseconds per step. Beside that, this value range fits quite well within a byte.

Given that pinguino uses a 20 Mhz xtal, and it is configured for using USB, the PLL will be needed, and will multiply the frecuency to produce a CPU clock of 48 Mhz. which also means 12 MIPS ( = Fosc / 4 ). Thus, every milisecond 12500 instructions are executed or 12500 ticks will be elapsed whe TMR1 uses the internal oscillator for timings. In the other hand, during 4 microseconds 50 instructions will be executed, which should be also taken into account.

Once explained that, it only left to plan how and when generate interrupts:

1. A interruption will be generated to set all servo pulses up (high level), and configure TMR1 to allow a 1 milisecond delay.
2. Once that milisecond has elapsed, a new interruption must be generated, and directly go to run the routine to set the servos down at their specific timings. This routine will last 1 ms exactly. During that time we stay within interrupt routine (is the cost of doing in this way). After finishing that, we set up TMR1 again, but now to generate a new interrupt after 18 ms, and start it. But before going out from the interrupt routine, we check the flag that let us know if some servo value has been modified (by the function SetServo ) and then it is needed to run the sorting routine again. If needed, we will consume more time within the interrupt routine (although 18 ms timer is already running), but we make sure that the timings array is ready for all the incomming cycles. When finished everything it goes out from interrupt routine to the main flow, so the main program will continue running. This is like that up to the moment that 18ms have elapsed and a new interrupt will be generated. At that moment it will start at point 1 again.

So simple like that.

For managing which servos are active, and which servos should be set down when requiered, we store masks of the every ports bits. In that way, we only need logical operators like AND (to set servo pulses to high) and XOR ( to set servos pulses to low) to modify a specific bit value without disturbing the rest of the port bits.

The routine below is commented in english, so it should be easy to understand.

Now the target is to make a pinguino library with it.

If someone have ideas to improve, they will be welcomed !!!!

Here is the code. For copying it used buttons that will apear in the code box (upside-right corner) when you pass mouse pointer over it. Only to say that this routine was created, compiled and tested with pinguino beta 8 suite :

// ------------------------------------------------------------------------------
// UP TO 18 SERVOS CONTROL ROUTINE FOR PINGUINO 18F2550
// ==============================================================================
// Version: 1.0
// Author:  Jesús Carmona Esteban (Sphinx for ARDE: Asociacion Robotica y Domotica de España)
// Date:  28/7/2010
//
// This routine has been done and tested for Pinguino 18F2550
// it allows to control up to 18 servos simultaneously. So, you can activate all
// Pinguino pins for servo driving.
// NOTES:
// -  Xtal is 20 Mhz, so the PLL output will provide a CPU clock of
//    48Mhz => 12 MIPS (Fosc/4).
// - Used resources:
//        * Timer TMR1
//        * Memory used: 98 postions aproximately
// - Resolution for every servo positioning is given with values from 1 to 250.
//   So, a minimum resoltion of 4 microseconds.
// - The function for servo setting is (servo,value).
// - All servos are managed in parallel by the PIC. This means that
//   all pines activated as servo controllers are set as high at the same time, and
//   will be set down at their respective moments (according to the value set by
//   SetServo function).
//
//
// Future improvements:
// ------------------
// - Optimizar la rutina de ordenación. Actualmente tarda 1,64 ms en ejecutarse
//    para ordenar los 18 servos.
// - Adaptar la rutina para que sirva para el 18F4550 también.
// -----------------------------------------------------------------------------

#define PIC18F2550

// Minimum and Maximum values allowed for setting a servo.
#define SERVOMAX 250
#define SERVOMIN   1

//variables
uchar phase=0;
uchar needreordering=0;
uchar timingindex;
uchar timedivision=0;
uchar loopvar;
uchar timings[4][18];  // Bidimensional matrix for timings and masks associanted alocation.
uchar activatedservos[3]={0x00,0x00,0x00};  // Matrix contaning masks of pinguino pins activated as servos. Bytes are B,C and A respectively.
// Indexes values for referencing in previous matrix.
#define MaskPort_B  0
#define MaskPort_C  1
#define MaskPort_A  2
#define timevalue   3

uchar servovalues[18]; // Array containing values set for every pinguino servo pin.

// Array with all masks for all Pinguino 18F2550 pins:
const uchar servomasks[18]={0X01,0x02,0x04,0x08,0x10,0x20,0x40,0x80,0x40,0x80,0x01,0x02,0x04,0x01,0x02,0x04,0x08,0x20};

//miscelanea
uchar valor=1;

void ServosPulseDown()
{
 timingindex = 0;

 for(timedivision=0;timedivision < 251;timedivision++){
 if (timings[timevalue][timingindex] == timedivision){
 PORTB = PORTB ^ timings[MaskPort_B][timingindex];   // XOR of Port and mask...
 PORTC = PORTC ^ timings[MaskPort_C][timingindex];   // ...it will set low level correspondent pins.
 PORTA = PORTA ^ timings[MaskPort_A][timingindex];
 timingindex++;
 }
 // The following piece of ASM code provides a precise delay to make every loop in this for lasts aprox. 4 microseconds.
 __asm
 movlw 6
 movwf _loopvar
 bucle:
 NOP
 decfsz _loopvar,1
 goto bucle
 __endasm;
 }
}

void ServosPulseUp()
{
// This function sets high level all pins configured as servo pins.
 PORTB = activatedservos[MaskPort_B] & 0xFF;
 PORTC = activatedservos[MaskPort_C] & 0xFF;
 PORTA = activatedservos[MaskPort_A] & 0xFF;
}

void SortServoTimings()
{
// This function reads the 'servovalues' array and sort timing values in 'timings' array
// from lesser to greater, association to every entry the servos that should be set down at
// that specific timing.

 uchar s,t,totalservos,numservos;
 uchar mascaratotal[3]={0x00,0x00,0x00};

 // array initialization:
 for(t=0;t<18;t++){
 timings[timevalue][t]=255;
 timings[MaskPort_B][t]=0x00;
 timings[MaskPort_C][t]=0x00;
 timings[MaskPort_A][t]=0x00;
 }

 totalservos=0;
 t=0;
 while(totalservos<18) {
 numservos=1;
 for(s=0;s<18;s++) {
 // Case of pins associated to port B:
 if (s<8){
 // The following if checks that the pin is active as a servo and if it has been previously checked in a previous loop.
 if (servomasks[s] & mascaratotal[MaskPort_B] & activatedservos[MaskPort_B]){
 }
 else if (servovalues[s] < timings[timevalue][t]){
 timings[timevalue][t]=servovalues[s];
 timings[MaskPort_B][t]=servomasks[s];
 timings[MaskPort_C][t]=0x00;
 timings[MaskPort_A][t]=0x00;
 numservos=1;
 }
 else if (servovalues[s] == timings[timevalue][t]){
 timings[MaskPort_B][t] |= servomasks[s];
 numservos++;
 }
 }
 // Case of pins associated to port A:
 else if (s>12){
 // The following if checks that the pin is active as a servo and if it has been previously checked in a previous loop.
 if (servomasks[s] & mascaratotal[MaskPort_A] & activatedservos[MaskPort_A]){
 }
 else if (servovalues[s] < timings[timevalue][t]){
 timings[timevalue][t]=servovalues[s];
 timings[MaskPort_B][t]=0x00;
 timings[MaskPort_C][t]=0x00;
 timings[MaskPort_A][t]=servomasks[s];
 numservos=1;
 }
 else if (servovalues[s] == timings[timevalue][t]){
 timings[MaskPort_A][t] |= servomasks[s];
 numservos++;
 }
 }
 // Case of pins associated to port C:
 else {
 // The following if checks that the pin is active as a servo and if it has been previously checked in a previous loop.
 if (servomasks[s] & mascaratotal[MaskPort_C] & activatedservos[MaskPort_C]){
 }
 else if (servovalues[s] < timings[timevalue][t]){
 timings[timevalue][t]=servovalues[s];
 timings[MaskPort_B][t]=0x00;
 timings[MaskPort_C][t]=servomasks[s];
 timings[MaskPort_A][t]=0x00;
 numservos=1;
 }
 else if (servovalues[s] == timings [timevalue][t]){
 timings[MaskPort_C][t] |= servomasks[s];
 numservos++;
 }
 }

 }
 mascaratotal[MaskPort_B] |= timings[MaskPort_B][t]; // This mascaratotal accumulates all the values
 mascaratotal[MaskPort_C] |= timings[MaskPort_C][t]; // of all active servos already revised
 mascaratotal[MaskPort_A] |= timings[MaskPort_A][t]; //
 totalservos += numservos;
 t++;

 }
 needreordering=0;  // This flag says that timings array is already sort.
 // So, no need of run this function up to a value of a servo will be changed.
}

void EnableTimerInterrupt()
{
 TMR1H=0xFF;
 TMR1L=0x00;
 // timer 1 prescaler 1 source is internal oscillator
 T1CON=0x01;
 // enable interrupt for timer1 in register PIE1
 PIE1bits.TMR1IE=1;
 // enable peripheral interrupt
 INTCONbits.PEIE=1;
 // global enable interrupt
 INTCONbits.GIE=1;
}

void SetServo(uchar servo, uchar value)
{
 if(servo>=18)        // test if numservo is valid
 return;

 if(servo<8){
 activatedservos[MaskPort_B] = activatedservos[MaskPort_B] | servomasks[servo];
 } else if (servo>12) {
 activatedservos[MaskPort_A] = activatedservos[MaskPort_A] | servomasks[servo];
 } else {
 activatedservos[MaskPort_C] = activatedservos[MaskPort_C] | servomasks[servo];
 }

 if(value<SERVOMIN)  //  1 = 1000 usec pulse
 value=SERVOMIN;
 if(value>SERVOMAX) // 250 = 2000 usec pulse
 value=SERVOMAX;
 servovalues[servo]=value;

 needreordering=1;  // Set the flag in order the array timings will be reordered.
}

//interrupt handler that handles servos
void UserInterrupt()
{
 if (PIR1bits.TMR1IF) {
 PIR1bits.TMR1IF=0;
 T1CON=0x00;
 if (phase) {
 //Case for the 1st ms. All servo pulses are set up high.
 ServosPulseUp();
 // Load in TMR1 53035d (also 0xFFFF - d'12500) 12500 TMR1 ticks (using internal oscillator) = 1ms.
 TMR1H= 0xcf;
 TMR1L= 0xb2;
 // timer 1 prescaler 1 source is internal oscillator  = 48Mhz >> 12 MIPS
 T1CON=1;
 phase = 0;
 } else {
 //caso antes 2do ms:
 // The following function takes 1 ms
 ServosPulseDown();
 // Now we launch a 18 ms timer.
 // Load in TMR1 9285d (also: 0xFFFF - [(4,5 x 12500(duration 1ms)) = 0x2445] => 4,5 ms)
 // With this value and preescaler x 4, we'll get 18 ms delay.
 TMR1H= 0x24;
 TMR1L= 0x45;
 if (needreordering)    // If need reordering the sort function will be called.
 SortServoTimings();
 // timer 1 prescaler 1:4 source is internal oscillator  = 48Mhz >> 12 MIPS
 T1CON= ( 1 | 2 << 4 ) ;
 phase = 1;
 }
 return;
 }
}

void setup()
{
unsigned char a;

// For the demo:
//All Pins set up as outputs.
for(a=0;a<18;a++) {
 pinMode(a,OUTPUT);
 servovalues[a]=255;
 }

//For the demo:
//Here we set all pins as servo drivers, using different values:
//( you can play with them)
SetServo(0,1);
SetServo(1,20);
SetServo(2,30);
SetServo(3,110);
SetServo(4,123);
SetServo(5,200);
SetServo(6,21);
SetServo(7,41);
SetServo(8,251);
SetServo(9,61);
SetServo(10,254);
SetServo(11,11);
SetServo(12,133);
SetServo(13,199);
SetServo(14,90);
SetServo(15,21);
SetServo(16,1);
SetServo(17,100);

SortServoTimings();     // We launch a first reordering before
EnableTimerInterrupt(); // ...enabling interruptions.
}

void loop()
{
 // For the demo:
 // With this loop we will make that servos 0,2,10 y 17 v change along the time.
 delay(250);
 SetServo(0,valor);
 SetServo(2,255-valor);
 SetServo(10,valor);
 SetServo(17,255-valor);
 valor++;
}