Organizar datos metodo Bubble Sort para n servos

[AKENAFAB]

Hola amigos

Realice este programita para organizar de menor a mayor la posicion de los servos, esto para una controladora de "n" servos.
Por ahora organiza bien xD falta todo lo demás por eso lo pego aqui , despues probaré por metodo de pivote que es más rápido.

Deje valores al azar en posicion inicial de los servos.Ahi se almacenaran las posiciones que lleguen por serial.
Una vez se tengan todas posiciones el programa ira actualizando la lista.

Código: C

1. #include <18f252.h>
2.  
3. //#DEVICE ADC=8        //Solo regresa 8bits mas altos de la conversion
4.  
5. #FUSES H4,NOWDT,PUT,NOBROWNOUT,NOLVP,NOPROTECT,NODEBUG,CCP2C1,NOOSCSEN
6. #use delay(clock=40MHz)
7. #use rs232(uart1,baud=9600)
8.  
9. #USE FAST_IO(A)
10. #USE FAST_IO(B)
11.  
12. ////////////////////////////////////////////////////////////
13. //   Control de ServoMotor mediante algoritmo burbuja     //
14. ////////////////////////////////////////////////////////////
15.  
16. ///------ - Tarjeta - -------   ///
17.  
18. ///
19. /////////////////////////////////////////////////////////
20. //    - algoritomo burbuja                             //
21. //    - Controladora de servomotores                   //
22. //                                                     //
23. //                                                     //
24. /////////////////////////////////////////////////////////
25.  
26.  
27. #zero_ram                                  //Inicializa variables a 0
28.  
29.  
30. #define n_servos  16                       //Numero de servos
31.  
32. char servo_motor[n_servos]={0,1,2,3,4,5,6,7,8,9,10,11,12,13,14};              //Servos numerados
33.  
34. char servo_grados[]={ 10, 178, 96, 51,89, 13, 5, 36, 9,80,20,70,90,80,178,180 };  //Valor inicial de los servos
35.  
36. void bubbleSort(int grados[], int num_servos);
37.  
38. char servos;           //Variables usadas para impresion de datos
39.  
40.  
41. void main(){
42.  
43.             output_a(0b00000000);                     //Latch puertos
44.             output_b(0b00100000);                     //Latch puertos
45.            
46.            
47.             set_tris_a(0b00000000);             //Config Puertos ADC0=A0
48.             set_tris_b(0b00000100);             // PWM por B0,TX=B5,RX=B2
49.            
50.            
51.            
52. ////////////// Programa   ////////////////////////
53. while(1){
54.  
55. bubbleSort(servo_grados,n_servos);  //funcion burbuja
56.  
57. for(servos=0;servos<n_servos;servos++){
58.  
59.    printf("Servo %2u = %3u grados\r",servo_motor[servos],servo_grados[servos]);
60.  
61. }
62.  
63. while(1); //loop
64.  
65.  
66. }//endwhile
67.  
68.    
69. }// en main
70.  
71. void bubbleSort(int grados[], int num_servos)
72. {
73.   int i, j, temp;
74.  
75.   for (i = (num_servos - 1); i > 0; i--)
76.   {
77.     for (j = 1; j <= i; j++)
78.     {
79.       if (servo_grados[j-1] > servo_grados[j])
80.       {
81.  
82.         temp = servo_grados[j-1];                 //Acomoda de menor a mayor
83.         servo_grados[j-1] = servo_grados[j];
84.         servo_grados[j] = temp;
85.        
86.         temp=servo_motor[j-1];               //Organiza servo con menor valor
87.         servo_motor[j-1]=servo_motor[j];
88.         servo_motor[j]=temp;
89.         //printf("numero = %u \r",numbers[j]);
90.       }
91.     }
92.   }
93. }

Cuando tenga más avances nos leeremos por aqui xD

Según mis pnsamientos xD tenemos 18ms -+ para realizar cualquier calculo , asumiendo con la spruebas realizadas que con 2ms es suficiente para todo el calculo , por ahora para 16 servos tarda 800us.Falta agregar los tiempos en los que se carga el timer,los tiempos de entrad ay salida de interrupcion ,el tiempo del manejo del dato , etc..