PicManía by RedRaven |
Búsqueda personalizada
|
CONTROLANDO
8
SERVOS
CON |
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 ... |
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. |
|
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. |
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: |
|
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. |
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. |
|
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. |
Implementación en CCS C: |
Para realizar la implementación de este algoritmo en C comenzaremos por
comentar algunos detalles importantes. |
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
|