PicManía by RedRaven
 

Búsqueda personalizada

 

CONTROLANDO 8 SERVOS CON
 UNA SOLA INTERRUPCIÓN

 
 

Un algoritmo para controlar hasta 8 servomotores del estilo de los Hitec HS300 HS311 o los Futaba 3003 mediante una sola interrupción dejando libre todo el main() para maniobrar a gusto.

 

Preámbulo:
 

   Por ahí tengo ya editados algunos artículos dedicados a este tema de los servos y los PWM's, concretamente recuerdo Controlando un SERVO con el PIC desde nuestro PC o el Proyecto Radiocontrol : Receptor RC asistido por PIC ...

   Todos ellos han tratado de un único servo, apuntando algunas ideas para poder manejar mayor cantidad de ellos, pero sin concretar nada sobre dicha posibilidad.  En cualquiera de los nuevos proyectos en los que me quiero meter se prevé manejar no uno sino muchos servos, así que ahora ha llegado el momento de acometer el tema.

   La idea de cómo hacerlo la he tomado prestada de la gente de IEArobotics y quiero dejar aquí constancia de ello y de mi público agradecimiento.
 

Introducción:
 

   No vamos a entrar en una descripción detallada de cómo manejar un servo, para ello a quien le interese puede visitar uno de los hilos anteriormente mencionados, pero vamos a dar al menos un somero repaso al tema.

Un servo, de los que usamos los hobbystas, se alimenta con unos 5V y se controla mediante una señal PWM con una frecuencia de 50 Hz, o sea con un periodo de 20 ms, y con un Duty Cycle que varía entre el 6% y el 15%, o sea entre un periodo en alto de unos 0.5 ms a 2.5 ms (estos valores pueden cambiar según el modelo concreto de servo que utilicemos, los Hitec van desde los 0.5 a los 2.5 ms y los Futaba desde los 0.3 a los 2.3 ms aproximadamente)

Con un pulso de unos 0.7 ms el servo se posiciona en un extremo de su recorrido, con un pulso de unos 2.3 ms el servo se posiciona en el extremo contrario y con un pulso intermedio, de unos 1.5 ms, se posiciona en el centro de su recorrido.

Aquí tenéis un cronograma de cómo reacciona un servo ante estos tipos de pulsos PWM:
 


 

   Implementar la generación de un solo pulso PWM no es cuestión difícil y en los artículos mencionados antes tenéis formas sencillas de hacerlo.

    La cosa sin embargo se complica conforme vamos acumulando una tras otra la necesidad de controlar mas y mas servos. Y todo es un problema de tiempos, conforme mas servos vamos intentando controlar nuestras rutinas se van complicando, ocupando mas tiempo de proceso, y empiezan a perder la precisión necesaria en la generación de los pulsos de cada uno de los servos. Éstos empiezan a temblar y a hacer cosas "raras", vibran, tiemblan, dan saltos ... y todo producido por una inestabilidad en los anchos de los pulsos que le llegan.

    Es mi intención en este hilo la de presentaros una forma económica en recursos y tiempos de proceso, en el PIC para controlar hasta 8 servos de forma estable y precisa, haciendo además uso de una única interrupción: La del desborde del Timer1 (rodando a 16 bits de precisión).
 

Descripción del algoritmo:
 

   Para describiros el algoritmo tenemos que hacer un uso intensivo de la calculadora. El primer dato a tener en cuenta es la frecuencia del pulso PWM de los servos, que como dijimos anteriormente es de 50 Hz. Esto significa que cada pulso a cada servo tiene un periodo, o tiempo que transcurre entre dos flancos sucesivos:

T= 1/F = 1/50 = 0,02 s = 20 ms

   Este periodo lo podemos dividir entre 8 para obtener lo que vamos a denominar una "ventana" para cada uno de los servos.

W = 20/8 = 2.5 ms

   Dentro de cada una de estas "ventanas" de tiempo de 2.5 ms de duración vamos a controlar un solo servo, pero uno tras otro hasta completar los 8 que son nuestro objetivo. Esto significa que independientemente del tiempo que esté en alto el pulso de un servo, recordad que decíamos que podía ser entre 0.7 ms y 2.3 ms, cada 2.5 ms ponemos en alto el siguiente servo, y como han transcurrido 2.5 ms desde la puesta en alto del anterior éste ya debe de haber sido bajado porque ningún pulso en alto puede sobrepasar los 2.3 ms.

Esto hace que dentro de cada 2.5 ms se pone en alto y después en bajo la señal de un servo. Como son 8 servos y por lo tanto 8 "ventanas" de 2.5 ms, a cada servo le tocará de nuevo ponerle la señal en alto 8 * 2.5 ms = 20 ms. O sea el periodo correspondiente a la frecuencia de 50 Hz que necesita.

Esto puede verse de forma mucho mas clara mediante el siguiente cronograma:
 


 

   Fijaos cómo las señales de cada uno de los servos se van generando una tras otra, de forma sucesiva, pero separadas cada una de la siguiente los mismos 2.5 ms hasta completar los 20 ms con los 8 servos, y cómo independiente de en qué momento se ponga en alto uno de ellos individualmente, a los 20 ms vuelve a tocarle a ese mismo ponerse en alto.

   Ahora la cuestión pasa por generar los "timmings" necesarios para ser capaces de producir este tren de pulsos de ancho controlado.
 

Recursos a utilizar:
 

   Como os puse mas arriba el principal recurso a utilizar es una única interrupción, la del desborde del Timer1. Pero antes debemos tratar otro tema relacionado con éste.

   Una idea que a nadie se le debe escapar, pero que por mas obvia que sea no voy a dejar de mencionar, es la de que hace falta un PIC razonablemente rápido. Con un pulso al extremo, de los 2.3 ms, vamos a tener sólo 0.2 ms para detectar el fin del pulso y ponerlo a bajo. No es mi intención calcular qué frecuencia FOSC mínima podríamos utilizar, he escogido un PIC que puede volar a 20 Mhz con el que tenemos mas que suficiente y si deseáis probar con frecuencias menores os rogaría que me hicieseis partícipe de los resultados.

   Con un cristal de 20 Mhz y el Timer1 configurado para como contador de 16 bits, con prescaler a 1:1 tenemos los siguientes "timmings" :
 


 

   El dato fundamental es el del tiempo de incremento de un Tick del Timer1, que resulta ser de 0.2 uS, o lo que es lo mimo 0.0002 ms.

   Podemos así expresar en esta unidad de Ticks los tiempos en alto de cada uno de los servos. Si todos tuviesen que estar en el centro de su recorrido sus PWM deberían de ser de 1.5 ms en alto:

Tx = 1.5ms/0,0002ms = 7.500 Ticks

   Así si habilitamos la interrupción por desbordamiento del Timer1 la primera vez que desborde ponemos en alto el pin correspondiente al Servo número 1 y precargamos el valor del Timer1 con 7.500 ticks antes del desborde, que se produce a los 65.535 ticks, con lo que ponemos el Timer1 a 65.535 - 7.500 = 58.035, de esta forma obtendremos la siguiente interrupción cuando transcurran esos 7.500 ticks.

   Cuando la interrupción salta de nuevo y entramos por segunda vez tenemos que prefijar entonces el tiempo que ha de transcurrir para completar la ventana de 2.5 ms hasta el comienzo del control del siguiente servo. Como en nuestro ejemplo hemos utilizado un valor de 1.5 ms ó 7.500 Ticks tendremos ahora que esperar 2.5 ms - 1.5 ms = 1 ms por lo que vamos a precargar de nuevo el Timer1 con 65.535 - 5.000 = 60.535 y nuestra interrupción saltará de nuevo transcurrido 1 ms.

   Ahora le toca al siguiente Servo con el que procederemos de la misma forma: ponemos en alto el pin del servo 2, precargamos el Timer1 restando al desborde en numero de ticks correspondiente a su ancho de pulso, esperamos la siguiente interrupción en la que ponemos en bajo el pin del servo y precargamos de nuevo con los ticks necesarios para completar los 2.5 ms de su ventana y de nuevo a empezar pero con el siguiente servo.

   Esta es la idea fundamental. Cada dos interrupciones se controla completamente un Servo y entre ambas transcurre exactamente 2.5 ms, independiente del estado en alto que tenga que tener cada servo, cada 16 interrupciones se vuelve de nuevo al principio, al primer servo, y han transcurrido exactamente 20 ms.

   Con una simple tabla de 8 enteros de 16 bits para guardar el número de ticks que tiene que estar en alto cada pulso de cada servo y con dos simples cálculos aritméticos de suma y resta en una única interrupción tenemos perfectamente controlados hasto 8 servos.
 

Implementación en CCS C:
 

   Para realizar la implementación de este algoritmo en C comenzaremos por comentar algunos detalles importantes.

   Vamos a usar la directiva #use fast_io(X) y fijar el funcionamiento de los pines con el set_tris_x(). Esto hace que el compilador no incluya los set_tris() automáticamente cada vez que se encuentra un output_low() o output_high() con lo que quitaremos instrucciones innecesarias de en medio y ganaremos en velocidad y precisión en nuestro programa.

   En el ejemplo que he preparado interactúo con el PIC mediante el canal serie del mismo con un MAX232. Así que he habilitado también la interrupción por recepción serie. Como los tiempos, "timmings" les llamábamos, son fundamentales en esta aplicación vamos también a usar la directiva #priority timer1,rda que nos va a hacer prioritaria la interrupción del timer1 sobre la de recepción serie. Ante la duda el Timer1 gana y nosotros con él en estabilidad y precisión.

   He creado una tabla int16 Servo_PWM[8] en la que guardo los Ticks de ancho que tienen que durar cada uno de los pulsos de cada uno de los servos. Son los necesarios para restárselos a 65.535 tras inicio de cada pulso y para sumárselos al ancho de la ventana tras su final para colocar la interrupción al inicio del siguiente pulso.

   Como el inicio de un pulso de servo siempre llega con una interrupción impar y el final de ese mismo pulso llega con la interrupción par me he creado un int1 flag_Phase que va a hacerme saber si estoy al comienzo o al final de cada pulso.

   Si estoy al final, flag_Phase=1, entonces incremento el número del siguiente Servo que me toca tratar y que será al que le tengo que levantar la señal en la siguiente interrupción. Si al incrementar el siguiente servo es mayor que el último vuelvo a ponerlo a 0 y recomienzo de nuevo con el primero.

El programa queda tal como sigue:
 

  Titulo  
 
#include <18f1320.h>
#fuses HS,NOMCLR,PUT,NOWDT,NOPROTECT,BROWNOUT,BORV45,NOLVP,NOCPD,NODEBUG,NOWRT
#use delay(clock=20000000)
#use rs232(baud=115200, xmit=PIN_B1, rcv=PIN_B4)
#use fast_io(A)
#use fast_io(B)

#priority timer1,rda

#define SERVO1 PIN_B5
#define SERVO2 PIN_B3
#define SERVO3 PIN_A0
#define SERVO4 PIN_A4
#define SERVO5 PIN_B2
#define SERVO6 PIN_A3
#define SERVO7 PIN_B0
#define SERVO8 PIN_B5

const int16 Ticks4Window = 12500; // PWM Window for servo = 2.5 ms x 8 = 20 ms
const int16 Ticks4Minimum = 3500; // PWM High for Minimum Position = 0.7 ms
const int16 Ticks4Center = 7500; // PWM High for Center Position = 1.5 ms
const int16 Ticks4Maximum = 11500; // PWM High for Maximum Position = 2.3 ms

static char command;
static int16 Servo_PWM[8]={Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center,0,0,0,0};
static int8 Servo_Idx=0;
static int1 SERVO1_ON=1;
static int1 SERVO2_ON=1;
static int1 SERVO3_ON=1;
static int1 SERVO4_ON=1;
static int1 SERVO5_ON=0;
static int1 SERVO6_ON=0;
static int1 SERVO7_ON=0;
static int1 SERVO8_ON=0;
static int1 flag_Phase;
static int16 Ticks4NextInterrupt=53036;

#int_rda
void serial_isr(void){

  if(kbhit()){
    command=getc();
  }
}

#int_timer1
void timer1_isr(void){

  if(flag_Phase==0){
    if(Servo_Idx==0 && SERVO1_ON) output_high(SERVO1);
    if(Servo_Idx==1 && SERVO2_ON) output_high(SERVO2);
    if(Servo_Idx==2 && SERVO3_ON) output_high(SERVO3);
    if(Servo_Idx==3 && SERVO4_ON) output_high(SERVO4);
    if(Servo_Idx==4 && SERVO5_ON) output_high(SERVO5);
    if(Servo_Idx==5 && SERVO6_ON) output_high(SERVO6);
    if(Servo_Idx==6 && SERVO7_ON) output_high(SERVO7);
    if(Servo_Idx==7 && SERVO8_ON) output_high(SERVO8);
    Ticks4NextInterrupt = 65535 - Servo_PWM[Servo_Idx];
    set_timer1(Ticks4NextInterrupt);
  }
  if(flag_Phase==1){
    if(Servo_Idx==0 && SERVO1_ON) output_low(SERVO1);
    if(Servo_Idx==1 && SERVO2_ON) output_low(SERVO2);
    if(Servo_Idx==2 && SERVO3_ON) output_low(SERVO3);
    if(Servo_Idx==3 && SERVO4_ON) output_low(SERVO4);
    if(Servo_Idx==4 && SERVO5_ON) output_low(SERVO5);
    if(Servo_Idx==5 && SERVO6_ON) output_low(SERVO6);
    if(Servo_Idx==6 && SERVO7_ON) output_low(SERVO7);
    if(Servo_Idx==7 && SERVO8_ON) output_low(SERVO8);
    Ticks4NextInterrupt = 65535 - Ticks4Window + Servo_PWM[Servo_Idx];
    set_timer1(Ticks4NextInterrupt);
    if(++Servo_Idx>7) Servo_Idx=0;
  }
  ++flag_Phase;
}

void pres_menu(void){

  printf("\r\nA 18F1320 listen on RS-232");
  printf("\r\nEight Servos Control Algorithm");
  printf("\r\n");
  printf("\r\n[?] This menu");
  printf("\r\n[I] All to Minimum");
  printf("\r\n[C] All to Center");
  printf("\r\n[X] All to Maximum");
  printf("\r\n[+] Step to front");
  printf("\r\n[-] Step to back");
  printf("\r\n\n>");
}

void main(void) {

  int1 valid_command;
  int8 i;

  disable_interrupts(global);
  setup_adc_ports(NO_ANALOGS);
  setup_adc(ADC_OFF);
  setup_counters(RTCC_INTERNAL,RTCC_DIV_2);
  setup_timer_0(RTCC_OFF);
  setup_timer_1(T1_INTERNAL | T1_DIV_BY_1);
  setup_timer_2(T2_DISABLED,0,1);
  setup_timer_3(T3_DISABLED);
  port_b_pullups(FALSE);

  set_tris_a(0b00000000);
  set_tris_b(0b00010000);

  output_low(SERVO1);
  output_low(SERVO2);
  output_low(SERVO3);
  output_low(SERVO4);
  output_low(SERVO5);
  output_low(SERVO6);
  output_low(SERVO7);
  output_low(SERVO8);

  delay_ms(1000);

  command='\0';

  enable_interrupts(int_rda);
  set_timer1(Ticks4NextInterrupt);
  enable_interrupts(int_timer1);
  enable_interrupts(global);

  pres_menu();

  do {
    // Comandos serie
    if(command!='\0'){
      command=toupper(command);
      valid_command=0;
      printf("%c\r\n>",command);

      if(command=='?'){
        pres_menu();
        valid_command=1;
      }
      if(command=='I'){
        printf("> All to Minimum\r\n>");
        for(i=0;i<4;i++) Servo_PWM[i]=Ticks4Minimum;
        valid_command=1;
      }
      if(command=='C'){
        printf("> All to Center\r\n>");
        for(i=0;i<4;i++) Servo_PWM[i]=Ticks4Center;
        valid_command=1;
      }
      if(command=='X'){
        printf("> All to Maximum\r\n>");
        for(i=0;i<4;i++) Servo_PWM[i]=Ticks4Maximum;
        valid_command=1;
      }
      if(command=='+'){
        printf("> Step to front\r\n>");
        for(i=0;i<4;i++) Servo_PWM[i]+=80;
        valid_command=1;
      }
      if(command=='-'){
        printf("> Step to back\r\n>");
        for(i=0;i<4;i++) Servo_PWM[i]-=80;
        valid_command=1;
      }

      if(!valid_command) printf("?\r\n>");
      command='\0';
    }
  } while (TRUE);
}
 
 
 

 

 
 
Mi montaje funcionando:
 
    Solo tengo 4 servos disponibles así que solo 4 le he conectado físicamente, pero el programa contempla los 8 aunque los 4 últimos no los secuencia en dos interrupciones sino que una de ellas salta inmediatamente y es la segunda la que hace todo el recorrido de la ventana de esos servos.
 


 

   Y este es el pequeño menú que me he preparado para controlar su funcionamiento:
 


 

   Os aseguro que los servos se desplazan suavemente, sin vibraciones ni temblores, entre ambos extremos siguiendo puntualmente las ordenes que les mando desde el PC.

   En este menú, como veis, manejo todos los servos a la vez, de hecho lo que hago es solo cargar la tabla Servo_PWM[] con los valores extremos y central y dejo que la interrupción haga el resto. Y la verdad es que va absolutamente de lujo.

Era una espinita que tenía clavada hace tiempo y que por fin he podido quitarme. Y quería compartirlo con todos ustedes.

Ea, ya está bien por hoy. Mañana más.

 

 

Esta página se modificó el 27/12/2008


Esta página usa la letra Ñ

Nota pública importante sobre las consultas al Webmaster de PicManía.


Sugerencias a Picmanía... (que serán leídas pero seguramente no podrán ser contestadas)

Esta página pertenece al grupo de páginas de Diego RedRaven

 

 



Nota: Esta página Web esta repleta de imágenes, textos, logotipos y demás material extraídos de los mas variados medios de los que no soy ni autor ni depositario de los correspondientes derechos de autor, uso y/o reproducción. Si Ud. es depositario de dichos derechos y desea que el material correspondiente sea eliminado de esta Web no dude en ponerse en contacto conmigo mediante e-mail y será inmediatamente retirado. Gracias.
 
Visitas
Totales : 16750 Hoy: 1 Activas: 1 Vistas: 16750

Esta página fue modificada el 07-08-2010 15:42:21

           
 DmSoft WAMP Escribir Unreal