CONTROL DE SERVOMOTORES Todo lo que necesitas saber para controlar servos.

Por: Jorge Arturo Rodrguez Hernndez Correo: george.manson.69@gmail.com

Que es un Servo? El servo es un pequeo pero potente dispositivo que dispone en su interior de un pequeo motor con un reductor de velocidad y multiplicador de fuerza, tambin dispone de un pequeo circuito que gobierna el sistema. El recorrido del eje de salida es de 180 en la mayora de ellos, pero puede ser fcilmente modificado para tener un recorrido libre de 360 y actuar as como un motor.

Como opera un Servo? El control de posicin lo efecta el servo internamente mediante un potencimetro que va conectado mecnicamente al eje de salida y controla un pwm (modulador de anchura de pulsos) interno para as compararlo con la entrada pwm externa del servo, mediante un sistema diferencial, y asi modificar la posicin del eje de salida hasta que los valores se igualen y el servo pare en la posicin indicada, en esta posicin el motor del servo deja de consumir corriente y tan solo circula una pequea corriente hasta el circuito interno, si forzamos el servo (moviendo el eje de salida con la mano) en este momento el control diferencial interno lo detecta y enva la corriente necesaria al motor para corregir la posicin.

Para controlar un servo tendremos que aplicar un pulso de duracin y frecuencia especficos. todos los servos disponen de tres cables dos para alimentacin Vcc y Gnd y otro cable para aplicar el tren de pulsos de control que harn que el circuito de control diferencial interno ponga el servo en la posicin indicada por la anchura del pulso.

Como controlar un servo?

Bueno se necesita una precisin casi perfecta, ya que si queremos exactitud necesitaremos mas velocidad, pero bueno la estrategia para controlar un servo es la siguiente es dividir el rango de operacin, Por ejemplo;

Si frecuencia necesaria es 50hz que equivale a 20mS dividimos esto entre el nmero de servos que necesitamos;

20mS / (numero de servos) = tiempo necesario para controlar los servos solicitados.

La desventaja de esta operacin es que solo podemos operar a 8 servos mximo.

20mS / 8 servos = 2.5mS tiempo necesario para controlarlo.

Para empezar a entender esto vamos a manejar simplemente 2 Servo.

Con la formula anterior hacemos lo siguiente;

20mS / 2 servos = 10mS tiempo necesario para controlar a cada uno de ellos. Tenemos un rango de operacin de 10mS para cada servo, cuando un servo inicia el la posicin neutral que hablamos de 1.5mS este valor lo restamos a los 10mS.

10mS-1.5mS = 8.5mS este valor nos servir para poder hacer la transicin al otro servo, y cuando pase al otro servo apagara el que estaba encendido y prendera el otro y este encender en este caso en la posicin neutral sea al puso de 1.5mS a este valor lo restamos de los 10mS. Y saldr otra vez 8.5mS, cuando pase este tiempo apagara este servo y prendera el otro y a pasado exactamente 20mS. Tendremos una frecuencia de 50Hz para cada servo.

Grafica de operacin de 2 servomotores, se observa que cada uno va tener una frecuencia de 50Hz.

Empecemos a programar en PIC C COMPILER

Bueno ya sabemos la idea como trabar con dos servos, pero har falta usar los recursos de para realizar dicha operacin. En este caso se quiso trabajar con un lenguaje de alto nivel que es el C. En este caso usaremos el programa CCS C. Anteriormente en el documento que sub a la pagina de www.forodeelectronica.com en el tema de Ccs c programa hechos en mplab se subi un archivo .pdf que explica como crear un proyecto en Mplab, el nombre del archivo era PROYECTOS CON PIC Y SIMULACION EN PROTEUS. Bueno empecemos a trasferir la idea anterior de cmo controlar los dos servos a este programa.

Primero que nada hay que tener Tres factores primordiales que son conocer al servo que vamos a usar, estos parmetros seria cual es el rango de operacin y la frecuencia necesaria de trabajo. En este caso se usara dos servomotores HITEC que son los mas conocidos entre los electrnicos. Las caractersticas de los motores HITEC se ven la tabla de arriba: Rango de operacin de 0.9mS -> 0 grados 1.5mS -> 90 grados y 2.1mS-> 180 grados

El segundo factor es que timer del microcontrolador hay que usar, como dije anteriormente que es ms preferible usar el timer1 ya que tendremos ms rango de operacin y ms exactitud.

Sabemos que el timer1 es de 16 bits sea tenemos un valor de 0 a 65536.

El tercer Factor es la velocidad que se valla a implementar en el PIC, si usamos un Cristal de 20Mhz, tendremos un ciclo de 0.2uS.Como estamos trabajando con servos que deben de recibir pulsos exactos para que giren a los grados deseados con un cristal de 20Mhz se acopla perfectamente a nuestro proyecto.

Ahora bien vamos a configurar el timer1 del PIC. Lo configuramos de tal forma que dos cuente por cada ciclo de reloj, en este caso quedo de la siguiente manera:

setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);

T1_INTERNAL= Lo referimos a que cuente los pulsos del reloj interno. 20Mhz/4= 5Mhz Los pulsos seran de 0.2uS.

T1_DIV_BY_1 = Y el preescalar es de uno, entonces le indicamos al PIC que cuente Los ciclos de reloj.

Para que el timer1 se desborde a los 1.5mS se necesita hacer la siguiente operacin.

Si 1.5mS / 0.2uS= 7500, esta cantidad lo restamos a 65536 = 58036, entonces Se necesita cargar el timer1 con un valor de 58036 para que se desborde.

SET_TIMER1(58036); Le indica que el pulso va durar en tiempo alto de 1.5mS

Ahora cuando se deborde hay que cargar el timer1 con el tiempo bajo; 20mS/2=10mS por servo 10mS-1.5mS=8.5mS tiempo bajo. Entonces: 8.5mS/0.2uS = 42500 Y restamos: 65536-42500 = 23036 Hay que poner ese valor en el timer1:

SET_TIMER1(23036); Le indica que el pulso de tiempo bajo va a durar 8.5mS antes que llegu el desbordamiento.

Cuando este ltimo se desborde, ah tendremos que cambiar de servo para encender el que sigue, y as seria para el otro. Y as tendremos un pulso de 20mS para cada uno.

PROGAMA PARA CONTROLAR 2 SERVOS

#include<16F628A.H> #fuses HS,NOWDT,NOLVP,MCLR,NOPROTECT

//HS: CRISTAL DE HIGH SPEED 20MHZ //NOWDT: NO SE UTILIZA EL PERRO GUARDIAN //NOLVP: SE DESACTIVA ESTA OPCION DE PROGRAMACION //MCLR:ACTIVAMOS EL MASTER CLEAN //NOPROTECT: NO SE PROTEGE EL CODIGO()

#use delay(clock=20000000) #use fast_io(B) PUERTO B

//CRISTAL DE 20MHZ // RAPIDA INTERVENCION DEL

//Variables

unsigned long tick; unsigned int SERVO=0; //NUMERO DE SERVO unsigned int PUERTO=6; //SE UTILIZA PARA EL PUERTO B short LAHL=1;

//CONFIGURACION DEL PIC

void config(void){

set_tris_b(0x00); setup_comparator(NC_NC_NC_NC);//Comparadores APAGADO setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); //Tick de 0.0002ms set_timer1(tick); enable_interrupts(INT_TIMER1);

disable_interrupts(INT_RDA); enable_interrupts(GLOBAL); } #INT_TIMER1 void Interrupcion_timer1(void){

if(LAHL==1){//SI ES PARA EL TIEMPO ALTO? ENTRA bit_set(*PUERTO,SERVO);//ESTABLECE EL PIN A PRENDER tick=58036;//TOMA LOS TICK NECESARIOS PARA TIEMPO EN ALTO LAHL=0;//LO CAMBIA PARA EL TIEMPO BAJO }else if(LAHL==0){ //SI ES PARA EL TIEMPO BAJO? ENTRA bit_clear(*PUERTO,SERVO);//PONE A BAJO EL PIN QUE CORRESPONDE SERVO++; //INCREMENTA PARA SIGUIENTE SERVO

if(SERVO>=2){//HA TERMINADO EN EL PUERTO B? SERVO=0; } tick=23036; LAHL=1; } set_timer1(tick); //ESTABLECE EL TIMER PARA MANTENER EL PUSLO ALTO //O BAJO PARA EL PIN QUE CORRESPONDA } void main(void){ //TOMA EL TICK PARA EL TIEMPO BAJO //LO CAMBIA PARA EL TIEMPO ALTO

config();

while(TRUE){}; }

SIMULACION EN PROTEUS

Como observamos el puslo del primer servo (amarillo), dura exactamente 20mS y el otro( azul) tambien. Los dos Servomotores estan posicionados a 90 grados.

Con una frecuncia de 50Hz.

Pero lo malo de este mtodo es que hay que programarlo el PIC, las veces que queramos para que gire los motores a la posicin deseada, es chiste es programar al microcontrolador para que nosotros podamos atravs de potencimetros, PC, u otra cosa que controle el giro del servo. En este caso queremos controlar el servo va Serial para manipular el giro de los servos, sin necesidad de andar programando muchas veces. Regresando a PIC C COMPILER, tendremos que usar dos interrupciones del microcontrolador, en este caso la interrupcin por recepcin del USART que contiene el PIC y la interrupcin por el timer1 que ya hemos explicado. En el USART del microcontrolador solo hay que recibir dos datos, que seria el primer byte recibido seria para elegir el servo, mientras que el segundo byte seria para la posicin del servo deseado.

Pero es primordial que la interrupcin el timer1 no sea interrumpida por otra cosa, en este caso se usar una instruccin para solucionar este problema:

#priority timer1,rda

Esta instruccin le decimos al PIC que pase lo que pase, se atienda primero la interrupcin del timer1, y as cuando se de la casualidad de que al mismo tiempo se produzca las dos interrupciones se valla primero al timer1 a atender.

*Nota: Se debe de usar el USART para usar la interrupcin de recepcin, ya que si usamos otros pines para este uso no se producir ninguna interrupcin.

Ahora bien, Qu hacemos con esos 2 bytes que recibimos? Primero que nada, el byte de posicin consta de 255 probabilidades, pero solo queremos 180, ya que este numero es el mximo de grados que debe de girar el servo. Si sabemos que el rango mnimo de operacin de servo HITEC es de 0.9mS a 2.1mS, entonces podemos sacar una operacin para poder manejar el byte de posicin.

Si decimos que:

0.9mS / 0.2uS = 4500 El valor obtenido es el mnimo rango que podemos restarle al timer, sea si restamos: 65536 4500 = 61036 Ese valor lo cargamos al timer1 y obtendremos un giro de 0 grados. Y el mximo valor para cargar al timer1 para que gire 180 grados seria:

2.1mS / 0.2uS = 10500 65536 10500 = 55036 Ese seria el valor para que gire 180 grados el servo. Bueno ahora el Rango de Operacin para poder trabajar el servo con el timer1 se sacara de la siguiente manera:

(Valor mximo de trabajo) (Valor mnimo de trabajo) = Rango el timer1

As:

10500 4500 = 6000 Rango de operacin.

Pero sabemos que el valor que arrojara el byte de posicin ser de 0 a 180, este valor es el que va estar variando. Pero antes debemos saber que cuanto varia por grado el rango de operacin, simplemente seria una rengla de tres simple:

Si 6000 = 180 y (Valor x por grado) =1 entonces si vara el valor mnimo de operacin:

(Valor de incremento por grado) = (Valor mximo de opereacion) x 1 grado 180 gradoa

Valor de incremento por grado = (6000) / 180 = 33.333333 = 34 *Se eligi 34 para que sea mas presidio a la hora se restar al 655536. El valor obtenido se refiere al incremento por grado, que tiene el valor mximo de operacin. sea si enviamos al PIC, un valor de 20 porque queremos que gire 20 grados entonces:

(20*34) = 680

Pero sabemos que el valor mnimo de trabajo es de 4500, entonces:

4500 + 680 = 5180 Este valor lo restamos de 65536;

65536 5180 = 60356 Este valor es el tiempo en alto que debe de durar el pulso para que gire 20 grados. Pero como estamos usando dos servos, anteriormente se haba obtenido un valor de 10mS para cada servo para poder operar y hacer el swicheo entre servos. Debemos restar el valor obtenido anteriormente con el valor de 10mS, sea:

10mS / 0.2uS = 50 000

4500 + 680 = 5180 valor obtenido para tiempo alto 50000 5180 = 44820 este es el valor en tiempo bajo solo se resta con 655536 44820 = 20716 este valor hay que crgalo al timer para que sea el valor de 10mS.

Y as seria para el otro servo. Los valor que tenga el byte de posicin va estar variando. Entonces se pude realizar una formula que se pueda aplicar a nuestro programita.

TIEMPO ALTO = 65536 ((POSICION*34) + 4500; TIEMPO BAJO = 65536 (50000 ((POSICION*34) + 4500));

Y as podemos implementarlo a nuestro programita que pueda actualizar la posicin de nuestros servos sin que pierda la frecuencia de cada uno. El primer byte recibido que seria el que elige al servo que vallamos a controlar, se implementa una arreglo para llevar acabo este mtodo.

unsigned long HIGH[]={0,0}; unsigned long LOW[] ={0,0};

Se nota que arreglo contiene dos posiciones para cada servo, como para el tiempo alto y bajo.

Entonces nuestra lgica de programacin seria de esta forma.

HIGH[NSERVO] = 65536 ((POSICION*34) + 4500; LOW[NSERVO]= 65536 (50000 ((POSICION*34) + 4500));

NSERVO = ES EL NUMERO DE SERVO A MANEJAR

Y as podemos cambiar la posicin del servo via rs232.

Programa que controla 2 Servos via rs232

#include<16F628A.H> #fuses HS,NOWDT,NOLVP,MCLR,NOPROTECT

//HS: CRISTAL DE HIGH SPEED 20MHZ //NOWDT: NO SE UTILIZA EL PERRO GUARDIAN //NOLVP: SE DESACTIVA ESTA OPCION DE PROGRAMACION //MCLR:ACTIVAMOS EL MASTER CLEAN //NOPROTECT: NO SE PROTEGE EL CODIGO()

#use delay(clock=20000000)//CRISTAL DE 20MHZ #use rs232(uart,baud=2400) #use fast_io(A) #use fast_io(B) #priority timer1,rda // RAPIDA INTERVENCION DEL PUERTO B

//Variables

unsigned long tick; unsigned long POSICION; unsigned int SERVO=0; //NUMERO DE SERVO unsigned int PUERTO=5; //SE UTILIZA PARA EL PUERTO B unsigned int NSERVO; short LAHL=1; short BANDERA=0;

unsigned long HIGH[]={58036,58036}; //A 90 GRADOS POR DEFAULT unsigned long LOW[] ={23036,23036};

//CONFIGURACION DEL PIC

void config(void){ set_tris_a(0x00); set_tris_b(0x02); setup_comparator(NC_NC_NC_NC);//Comparadores APAGADO setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); //Tick de 0.0002ms set_timer1(tick);

enable_interrupts(INT_TIMER1); enable_interrupts(INT_RDA); enable_interrupts(GLOBAL); }

#INT_TIMER1 void Interrupcion_timer1(void){

if(LAHL==1){

//SI ES PARA EL TIEMPO ALTO? ENTRA

bit_set(*PUERTO,SERVO);//ESTABLECE EL PIN A PRENDER tick=HIGH[SERVO];//TOMA LOS TICK NECESARIOS PARA TIEMPO EN ALTO LAHL=0;//LO CAMBIA PARA EL TIEMPO BAJO }else if(LAHL==0){//SI ES PARA EL TIEMPO BAJO? ENTRA bit_clear(*PUERTO,SERVO);//PONE A BAJO EL PIN QUE CORRESPONDE SERVO++;//INCREMENTA PARA SIGUIENTE SERVO if(SERVO>=2){ //HA TERMINADO EN EL PUERTO A? SERVO=0; } tick=LOW[SERVO];//TOMA EL TICK PARA EL TIEMPO BAJO LAHL=1;//LO CAMBIA PARA EL TIEMPO ALTO } set_timer1(tick); //ESTABLECE EL TIMER PARA MANTENER EL PUSLO ALTO //O BAJO PARA EL PIN QUE CORRESPONDA }

#INT_RDA void Interrupcion_X_recepcion(void){

disable_interrupts(INT_TIMER1);//DESACTIVA EL TIMER 1 if(kbhit()){ //HAY INFORMACION A CAPTAR

NSERVO=getc();//1ER BYTE PARA EL NUMERO DEL SERVO POSICION=getc();//2DO BYTE PARA LA POSICION } BANDERA=1; //ESTE PARAMETRO TE INDICA QUE SE A EFECTUADO UN CAMBIO

EN UN DE LOS//SERVOMOTORES set_timer1(get_timer1());//CUANDO SE SALIO DE LA RUTINASE QUEDO CON UN

enable_interrupts(INT_TIMER1);//CIERTO VALOR EL TIMER 1, ENTONCES LO TOMAMOS }

void main(void){

config();

while(TRUE){ if(BANDERA==1){ disable_interrupts(INT_RDA); HIGH[NSERVO]=65536-((POSICION*34)+4400); LOW[NSERVO]=65536-(50000-((POSICION*34)+4400)); BANDERA=0; enable_interrupts(INT_RDA); } } }

Software para controlar 2 servos Bueno ya tenemos el programa que va a contener el PIC, para controlar los 2 servos y mantener la frecuencia necesaria para su funcionamiento correcto. Pero ahora falta el software necesario para controlar los motores, el programa se va a crear en Microsoft Visual C# que es un programa para hacer aplicaciones. Empecemos a crear un proyecto; Abrimos el programa C# y haces click en file y luego new Project y deber a parecer una ventana:

Para cambiar el nombre de proyecto simplemente escribimos donde dice Name En este caso le pondre CONTROL DE 2 SERVOS V1.1

Como es una aplicacin simplemente que ya esta seleccionado por default, hacemos click OK Y listo, para empezar hacer el programa.

Para cambiar el encabezado de nuestra ventana, simplemente hacemos click donde esta text (circulo rojo) y listo. Agregar el serial

Buscamos el Serial Port y lo arrastramos a la ventana y deber aparecer abajo (circulo rojo) que fue agregado a nuestro programa. Como nuestro programa del pic, trabaja a una velocidad de 2400 baud hacemos click en el serialport agregado para buscar sus propiedades al lado derecho y cambiamos donde dice baud = 9600 por baud =2400 (circulo rojo)

Agregar los COM Primero agremagos un groupbox (circulo azul) y lo arrastramos a la ventana y cambiamos el nombre (circulo rojo)

Y luego agregamos combobox (circulo azul) y lo agregamos arriba de groupbox Y hacemos un clic en el combo box para editar su contenido(circuilo verde)

Y agregamos todos los COM que queramos

Y luego agregamos botones igual como se hizo anteriormente, cambiamos nombres, los acomodamos y listo.

Agregamos TRACKBAR Los trackbar nos servir para enviar los grados que queremos para los servos.

Lo agregamos igual como los anteriores. Agregamos tambien algunos textbox que nos serviran para visualizar la posicin que hemos enviado. Pero como los trackbar tienen por default un valor de 0 a 10, solo hay que cambiar de 0 a 180, en sus propiedades. Mas o menos debe de quedar asdebende de los gustos de cada quien.

HORA DE PROGRAMARLO Primero debemos de abrir el puerto y cerrarlo cuando debe, hacemos dos click en el boton que hemos creado Conectar. Y nos enviara directo a su funcion:

Y escribimos:

Aqu solo abrimos el puerto. Cambiar el COM Para cambiarlo solo hay que hacer dos click en combobox para programar su funcion:

Y ya podemos cambiar el COM que deseemos antes de conectarlo. Desconectar el Serial

Bueno ya tenemos configurado el puerto serial. Y a puede abrir y cerrar el puerto y elegir que COM a usar. Ahora es fijar los valores de trackbar para que aparesca en el textbox.

Se oberva que se agrega variables que representara la posicin de cada servo, y otra variable que representara el numero de servo.

Lo anterior lo hacemos otra vez ms para el otro servo. Enviar dato por el serial. Presionamos el boton que creamos que era 0 que es enviar para el servo1 la posicin puesta por el trackbar.

Se nota que x toma el valor de 0 y luego es convertido a byte para que el PIC lo tranforme tal como es. Tambien se nota que se convirti la posicin. Y as lo hacemos para el segundo y listo!!!!

Queda as nuestro programita!!!

Simulacion Completa Esquema en proteus del circuito:

Antes de simularlo con un programa de virutal serial port hacemos un enlace entre dos COMs virtuales.

Al simular en proteus el circuito se conectara en el COM que hallamos puesto en el COMPIM en proteus, y luego conectamos nuestro programita que hicimos en C# en el COM correspondiente.

Se ve que estan enlazados los dos programas entonces empezemos a mover los servos

Al mover las barras a los angulos deseados se nota que el motor gira a donde queremos sin perder la frecuencia de 50Hz.

Espero que hayan aprendido como hacer sus propios controladores para servos, esto se aplica mximo para 8 servos, solo hay que calcular y listo.

Versin 1.3 17/12/2009

HECHO EN MEXICO