(O todo lo que siempre quiso saber sobre el Servo y nunca se atrevió a preguntar)
Pero como decía Jack El Destripador: ¡Vayamos por partes!
Y para empezar un poco de teoria, que a ninguno de nosotros nos va a venir mal. Un
Servomotor es un cacharro, entre otros muchos, que puede manejarse inyectándole un señal
PWM.
Y si me preguntáis qué es esto os respondo que es un método de control que consiste en enviar un tren de pulsos, cada uno de ellos con un periodo de tiempo en alto, a 5V, y otro en bajo, a 0V; separados cada uno del siguiente un tiempo constante y que podemos variarle la respectivas duraciones que permanece en alto y bajo, o como su propio nombre indica:
Pulse
Width
Modulation, que dicho para entendernos significa Modulación de Ancho de Pulso.
Un servo es un motor controlado por una electronica que lee el PWM y que se encarga de mover al motor dependiendo de lo que ha leído.
El servo, o mejor dicho la electrónica del servo colococa al motor en cada posición dependiendo del tiempo en que el pulso que le inyectamos permanece en alto. Si el tiempo que dura en estado alto dura exactamente 1.5 milisegundos entonces el Servo va y se coloca en el centro de su recorrido, si dura exactamente 0.5 milisegundos el servo retrocede desde el punto medio unos 90º y se coloca en su extremo izquierdo y si, por último, dura exactamente 2.5 milisegundos el servo avanza desde el punto medio unos 90º y se coloca en su extremo derecho. Al tiempo en que permanece en alto un pulso le llamamos
Duty Cicle.Con duraciones intermedias del tiempo en que permanece el pulso en alto, o Duty Cicle, el servo se posiciona en puntos intermedios de su recorrido.
Para que el servo responda correctamente a estos distintos Duty Cicles los pulsos deben llegarle al servo con una periodicidad, o
frecuencia constante, uno tras otro, separados 20 milisegundos cada uno uno del siguiente, cada flanco de subida debe estar separado del siguiente flanco de subida los mismos 20 milisegundos; por lo tanto cada ciclo alto-bajo dura siempre exactamente 20 milisegundos y lo que variamos es la relación entre el tiempo que está en alto y en bajo.
Decir que los pulsos estan separados unos de otros 20 milisegundos es exactamente lo mismo que decir que se envían con una frecuencia de 50 Herzios, ya que 50 hz son 50 pulsos por segundo y por lo tanto 1000 milisegundos (que tiene un segundo) dividido entre 50 son exactamente eso: 20 milisegundos. O sea aplicamos la formula
f (frecuencia en Herzios) =
1 / t (Periodo en Segundos).
En el fondo todo este asunto no es distinto de encender y apagar nuestro famoso led, que es algo por lo que empezamos todos cuando comenzamos a trastear con los PIC"s, pero controlando muy exactamente los tiempos durante los que que está encendido y apagado.
Esto podemos verlo de forma mas fácil y clara en la imagen esquema siguiente:
Ahora lo que tenemos que hacer es saber cómo podemos controlar estos tiempos en nuestro PIC para poner en alto (disparar el pulso) y en bajo (apagarlo) con la cadencia adecuada, siguiendo la tabla de tiempos descrita mas arriba.
Para ello voy a echar mano del socorrido
TIMER0 del PIC que me va a servir de reloj para saber cuándo y durante cuánto tiempo tengo que tener mi pulso en alto. Como soy el mas listo de la clase he elegido un divisor, o preescaler, del TIMER0 de 1:16 (mas adelante os contaré el por qué de este divisor).
Asumiendo que tenemos nuestro PIC funcionando con un cristal de 4.00Mhz entonces el TIMER0 funcionando a 1:16 hace saltar la
Interrupción por Desbordamiento de Timer, tambien conocida como
RTCC, cada 4.096 milisegundos.
Esto es lo mismo que decir que TIMER0 tarda 4.096 milisegundos en contar desde 0 a 255 y que al llegar a 255 pasar de nuevo a 0 hace saltar la RTCC.
Esto significa que cada paso de contador del TIMER0, a lo que llamamos un
tick de reloj, tarda 4.096 / 256 = 0.016 milisegundos. Esto me da una pauta bastante facil de calcular que consiste en que cada 5 RTCC completas tengo 5 * 4.096 = 20.48 milisegundos que es un poco más de lo que necesito, que son 20 milisegundos exactos:
Esto lo podemos conseguir contando 4 RTC"s completas, a 4.096 milisegundos cada una, y otra más un poco mas corta. No podemos hacer que la RTCC se acabe antes de la cuenta, pero si que podemos, y es lo que vamos a hacer, que empiece a contar un poco mas tarde, que no empiece a contar desde 0 sino desde 30: esto se explica porque 30 * 0.016 = 0.48 milisegundos menos que va contar esta última RTCC, al haber empezado desde un valor de 30 en lugar de 0, luego 4.096 - 0.48 = 3.616 milisegundos para la última RTCC.
Concluyendo: tengo 4 RTCC"s completas a 4.096 milisegundos y una capada a 3.616 luego 4 * 4.096 + 3.616 = 20 milisegundos. Lo que realmente voy a hacer es contar 1 RTC completa, 2 RTCC completas, 3 RTCC completas, 4 RTCC completas yy pongo el contador de TIMER0 a 30, y por fin 5 RTCC completas luego ya han pasado 20 milisegundos exactamente. Ya tenemos fijada la frecuencia que necesitamos.
A estas 5 RTCC"s les llamo
flagRTCC que sólo voy a activar cuando se completen las 5 RTCC"s (4 completas y otra mas incompleta).
Además sabiendo que cada tick de reloj ocupa 0.016 milisegundos podemos traducir los tiempos de anchos de pulsos descritos anteriormente en ticks de nuetro reloj particular: así 0.5 milisegundos son lo mismo que esperar 31 ticks de reloj, 1.5 milisegundos equivalen a 93 ticks de reloj y 2.5 milisegundos son 155 ticks de reloj. (Recordad que llamamos tick de reloj al tiempo que tarda TIMER0 en contar 1 más).
La imagen anterior podemos ahora convertirla en esta siguiente en la que hemos cambiado los tiempos por
RTCC"s y Ticks:
Como vemos en el cronograma superior: cada vez que se produce un super RTCC, de 4 RTC"s y pico a la que llamamos
flagRTCC, ponemos en alto el
PIN de la señal del Servo, debemos recordar que esto siempre va a ocurrir en el tránsito del contador TIMER0 entre los valores de 255 y 0, por lo que
flagRTCC siempre va a coincidir con TIMER0=0.
Ahora entonces solo debemos esperar el número suficiente de
ticks para volver a poner nuestro pin a bajo. Si deseamos que el Servo se posicione en su centro debemos mantener el
PIN en alto durante 93 ticks de TIMER0 o, lo que es lo mismo, esperar 1.5 milisegundos para bajar el pulso.
La secuencia queda entonces de la siguiente manera:
-
RTCC corre alocadamente, una tras otra, dedicandose exclusivamente a contar cuántas de ellas han pasado; si es la
cuarta pone TIMER0 a
30 para que la quinta sea mas corta, si es la quinta pone en alto
flagRTCC para lo que sea necesario y comienza de nuevo.
- En el programa principal detectamos que
flagRTCC se ha activado así que lo desactivamos y ponemos en alto el
PIN y marcamos, con
flagSERVO1, que acabamos de activarlo.
- A continuación, y siempre que
flagSERVO1 esté activado, comprobamos el valor de
TIMER0 que si es mayor que el que deseamos, en nuestro caso 93, y cuando lo alcancemos ponemos a bajo el
PIN y lo marcamos desactivando
flagSERVO1. Y hemos acabado.
Cada 20 milisegundos activamos el pulso, y transcurridos 1.5 milisegundos lo desactivamos, que es exactamente lo que queríamos hacer.
El valor de TIMER0, con el comparamos para controlar la duración de cada pulso, la tenemos guardada en
tSERVO1, que inicialmente cargamos con el número de ticks necesarios para colocar el Servo en su punto medio,
ticks_PULSO_MEDIO. El valor de
tSERVO1 lo podemos cambiar dinámicamente mediante la recepción de comandos a traves de la
RS232.
De esta forma con los comandos
"1", "2" y "3" podemos cambiar el valor de
tSERVO1 a
ticks_PULSO_MINIMO,
ticks_PULSO_MEDIO y
ticks_PULSO_MAXIMO respectivamente; y con los comandos
"+" y "-" vamos incrementando o decrementando su valor. Con
"r" le pedimos al PIC que nos envíe su valor actual.
Ahora solo nos quedaría implementar todo esto en un programa en C que queda de la siguiente forma:
Codigo:
// servo_pwm_232
// Ejemplo con un servo FUTABA S3003
// Alimentación y pulsos a 5V
// Cuadro de Tiempos :
// Periodo 20 ms (Frecuencia 50 Hz)
// Ancho Pulso minimo 0.5 ms
// Ancho pulso medio 1.5 ms
// Ancho pulso maximo 2.5 ms
// TMR0 a 1:16 -> 1 RTCC cada 4.096 ms
// -> 1 Tick cada 0.096 / 256 = 0.016 ms
// -> 20 ms = (4 x RTCC completas) + (1 * RTCC - 30 ticks)
// Ancho Pulso minimo 0.5 ms -> 31 ticks de TMR0
// Ancho pulso medio 1.5 ms -> 93 ticks de TMR0
// Ancho pulso maximo 2.5 ms -> 155 ticks de TMR0
#include <16f876a.h>
#fuses XT,NOWDT,NOPROTECT,NOLVP,PUT,BROWNOUT
#use delay(clock=4000000)
#use standard_io(b)
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7)
#define PIN_SERVO1 PIN_B0
const int AJUSTE_FINO_DE_RTCC = 30;
const int ticks_PULSO_MINIMO = 31;
const int ticks_PULSO_MEDIO = 93;
const int ticks_PULSO_MAXIMO = 155;
int1 flagRTCC = 0;
int contRTCC = 0;
int1 flagSERVO1 = 0;
int tSERVO1 = ticks_PULSO_MEDIO;
char Keypress =0x00;
void eco_servos(void);
void ajusta_servo(void);
#int_rda
void rda_isr() {
Keypress=0x00;
if(kbhit()){
Keypress=getc();
}
}
#int_RTCC
RTCC_isr(){
++contRTCC;
if(contRTCC==4){
set_TIMER0(AJUSTE_FINO_DE_RTCC);
}
if(contRTCC==5){
flagRTCC=1;
contRTCC=0x00;
}
}
void main() {
int ValTIMER0;
setup_counters(RTCC_INTERNAL,RTCC_DIV_16);
enable_interrupts(int_rda);
enable_interrupts(global);
printf("
SERVO Commander
" );
eco_servos();
set_TIMER0(0);
enable_interrupts(INT_RTCC);
do {
// DISPARO DEL PULSO PWM
if(flagRTCC==1){
flagRTCC=0;
output_high(PIN_SERVO1);
flagSERVO1=1;
}
// CONTROL DE ANCHO DEL PULSO PWM
if(flagSERVO1==1){
valTIMER0 = get_TIMER0();
if(valTIMER0>tSERVO1){
flagSERVO1=0;
output_low(PIN_SERVO1);
}
}
// CONTROL DESDE LA RS-232
if(Keypress!=0x00){
ajusta_servo();
Keypress=0x00;
}
} while (TRUE);
}
void ajusta_servo(void){
switch(Keypress){
// Periodos Prefijados
case "1": tSERVO1=ticks_PULSO_MINIMO;
break;
case "2": tSERVO1=ticks_PULSO_MEDIO;
break;
case "3": tSERVO1=ticks_PULSO_MAXIMO;
break;
// Inc Dec Periodo
case "+": if(++tSERVO1>ticks_PULSO_MAXIMO){
tSERVO1=ticks_PULSO_MAXIMO;
}
break;
case "-": if(--tSERVO1<ticks_PULSO_MINIMO){
tSERVO1=ticks_PULSO_MINIMO;
}
break;
// Dame Periodo actual
case "r": eco_servos();
break;
}
}
void eco_servos(void){
printf("S=%u
",tSERVO1);
}
Descargar código
aquiComo podéis ver en el comentario inicial del código, todo esto está montado para la familia de servos compatibles
FUTABA S3003, HiTec HS-300 CW, HOBBICO COMMAND CS-51, que son de los mas usados por los aficionados al Radiocontrol.
Para ajustar este código a otros servos solo hay que calcular el preescaler y los ticks necesarios para ajustarse a las caracteristicas de éste. Hay servos que funcionan a 400hz en lugar de 50hz y con anchos de pulso ligeramente distintos a los utilizados aquí. Es normal anchos de pulso en los extremos de 1.00 y 2.00 milisegundos respectivamente. El punto medio en 1.5 milisegundos es muy común.
Otro corolario de este ejemplito estriba en la posibilidad de manjear varios servos. Utilizando la misma estructura de
PIN_SERVO1 y tSERVO1 podemos habilitar el control indistinto de tantos servos como deseemos, teniendo así PIN_SERVO2 y tSERVO2, PIN_SERVO3 y tSERVO3 ... etc controlando cada uno de ellos de forma absolutamente similar.
Espero que os guste.
Aqui para ver la imagen en grande.
P.D. Muchas gracias al amigo
dogflu por sus inteligentes y acertadas observaciones sobre la redaccion de este ejemplito.
Autor