|
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
|