TODOPIC

Microcontroladores PIC => Lenguaje C para microcontroladores PIC => Mensaje iniciado por: ALE1973 en 27 de Abril de 2009, 00:09:54

Título: Algoritmo control 16 servos
Publicado por: ALE1973 en 27 de Abril de 2009, 00:09:54
Bueno, como Diego habia implementado el control de 8 servos, tome ese proyecto como base para hacer este para 16 servos, basicamente la teoria de funcionamiento es la misma, solo que por ventana de tiempo se controlan 2 servos a la vez, para resolver el problema que mencionaba Diego que si 2 servos debian bajar al mismo tiempo el pin que los controlan, lo que hice fue tener un registro con la imagen del puerto, la cual se actualiza al terminar la rutina de interrupcion, con lo cual los pines de los servos bajan efectivamente al mismo tiempo.
El programa utiliza 3 interrupciones, la de recepcion serie, la del timer 1 y la de comparacion.

Interrupcion recepcion serie: basicamente por lo que estuve probando no necesitas el pin DTR en la comunicacion serie, esto es, puedes enviar los datos en cualquier momento, tambien observe que se tienen mejores resultados a bajas velocidades de transmision, ya que no se llena el buffer de recepcion si no puedes ir a leer el byte recibido.
Interrupcion por timer 1: Esta interrupcion inicia cada pulso a los servos, y carga el tiempo que debe durar en alto el pin de cada servo, no necesitas los datos ordenados de mayor a menor o viceversa.
Interrupcion por comparacion: Esta es la interrupcion que resetea los pines de los servos y carga el tiempo hasta la proxima interrupcion por timer1 (2.5ms), y asi se vuelve a iniciar el ciclo.

En el programa que les dejo la frecuencia no es de 50 hz, si no menor, ya que con los servos que estuve probando obtengo mas fuerza y mejor presicion entre pasos, pero se puede modificar tranquilamente para tener 50hz.
Uso un pic 18f2550 con cristal de 20mhz y corre internamente a 48mhz, ya que la proxima es tratar de hacerlo por usb al control.
El algoritmo funciona sin saltos ni sobresaltos, solo que por ahora esta hecho para enviar de 0 a 100 la posicion del servo, luego tengo pensado modificar eso para que se puedan tener mas puntos intermedios.
Los datos desde el puerto serie es muy simple de enviar, simplemente envian 8 bytes con las posiciones de los servos, ojo solo de 0 a 100.
Este solo es el comienzo de un proyecto, asi que las criticas, modificaciones, etc que puedan hacer, aportar, con el fin de que mejoremos esto seran bienvenidas, los algoritmos que trate de encontrar para mas de 8 servos, todos son secretos, y pretendo que este NO SEA ASI, asi que sientanse libres de utilizarlo y mejorarlo (compartan las mejoras).

Saludos.
Alejandro.

Disculpen si la explicacion no es tan buena, ando con poco tiempo.
Título: Re: Algoritmo control 16 servos
Publicado por: Nocturno en 27 de Abril de 2009, 02:41:13
Ale, he estado mirando la rutina y, aunque no acabo de entenderla bien, me da la impresión de que resuelves la simultaneidad de movimientos sólo en 2 servos ¿Que ocurre si son más servos los que necesitan bajar a la vez?

Gracias por tu aportación
Título: Re: Algoritmo control 16 servos
Publicado por: ALE1973 en 27 de Abril de 2009, 08:55:20
Hola Nocturno, solo tratas 2 servos a la vez, ya que tienes 8 ventanas de tiempo por 2 en cada ventana son los 16 servos.

Código: CSS
  1. #int_timer1
  2. void timer1_isr(void){
  3. int16 tservo;
  4. int16 tservo1;
  5. tservo=Servo_Pos[Servo_nro];
  6. tservo1=Servo_Pos[Servo_nro+1];
  7. //    fServo1=0;
  8.     iguales=0;
  9.     bit_set(Port_Copy,Servo_nro);
  10.     bit_set(Port_Copy,Servo_nro+1);
  11.  
  12.     output_b((int8)Port_Copy & 0x00ff);
  13.     //output_D((int8)(Port_Copy>>8)& 0x00ff);
  14.     if(tservo>tservo1){
  15.             fServo1=1;
  16.             Ticks4NextInterrupt=tservo;
  17.             ccp_2=tservo1;
  18.             Tics2_5ms=65535 - Ticks4Window + tservo;
  19.     }        
  20.     else{
  21.             fServo1=0;
  22.             Ticks4NextInterrupt=tservo1;    
  23.             ccp_2=tservo;
  24.             Tics2_5ms=65535 - Ticks4Window + tservo1;
  25.     }
  26.     if(tservo1==tservo)flag_phase=1;
  27. //     #ASM bcf  0xd,0
  28. //          bcf  0xc,2
  29. //     #ENDASM  
  30.     set_timer1(0);
  31.     enable_interrupts(int_ccp2);
  32. }

Estas lineas se encargan de ir seteando los distintos pines de los puertos para comenzar el pulso de cada servo, la variable Servo_nro va incrementando su valor de 2 en 2 para manejar todos los servos.
Código: CSS
  1. bit_set(Port_Copy,Servo_nro);
  2.     bit_set(Port_Copy,Servo_nro+1);


Código: CSS
  1. #int_ccp2
  2. void ccp2_int(){
  3. int16 temp=0;
  4.    if(flag_phase==0){//baja primer servo
  5.       if(fServo1)     bit_clear(Port_Copy,Servo_nro);
  6.       if(!fServo1)    bit_clear(Port_Copy,Servo_nro+1);
  7.       CCP_2=Ticks4NextInterrupt;
  8.       flag_phase=1;
  9.    }      
  10.    ELSE{
  11.       OUTPUT_TOGGLE(PIN_A2);
  12.       Port_Copy=0;
  13.       set_timer1(Tics2_5ms);
  14.       if((Servo_nro+=2)>15)Servo_Nro=0;
  15.       disable_interrupts(int_ccp2);
  16.       flag_phase=0;
  17.    }
  18.    
  19.    output_b((int8)Port_Copy & 0x00ff);
  20.    // output_D((int8)(Port_Copy>>8)& 0x00ff);
  21. }

Estas lineas bajan el pin del servo correspondiente cuando corresponde (cuando finalizo el tiempo).
Código: CSS
  1. if(fServo1)     bit_clear(Port_Copy,Servo_nro);
  2.     if(!fServo1)    bit_clear(Port_Copy,Servo_nro+1);
 

Aca controlo si ya se manejaron los 16 servos, si es asi, reinicio el puntero para comenzar nuevamente

Código: CSS
  1. if((Servo_nro+=2)>15)Servo_Nro=0;

En esta version, si bien se estan controlando 16 servos, solo manejo 8 pines, ya que el 18f2550 no tiene mas puertos de 8 bits,
una desventaja de este metodo es que necesitas 2 puertos enteros de 8 bits, asi mueves directamente la copia del puerto al puerto.

Saludos, espero les sirva.
Alejandro

Editado: Gracias Diego por la rutina. Perdon por haber olvidado agradecer en el primer post.