Control de un brazo Robot de 4 ejes con PIC
Se trata del control de un brazo de 4 ejes para aplicar a un robot. El principio de funcionamiento es similar al empleado en el artículo Control de 8 servos con PIC.
Lo lógico es que la fuente de energía de los robots sean baterías o similares. Cuando intentamos hacer funcionar varios ejes del brazo a la vez, puede pasar que se produzcan bajadas de tensión momentaneas que ocasione el reset del PIC e impidan el movimiento del brazo. Para evitarlo hay que indicar en la configuración del programa:
#fuses NOBROWNOUT //No reset por baja tensión
Mediante interrupción por desborde del Timer 0 generaremos las señales de control de los servos. Y mediante el valor numérico contenido en una variable la posición de giro. Estas variables las hemos denominado ‘pinza’ , ‘muneca’ , ‘brazo’ y ‘hombro’, e identifican a cada uno de los ejes. Con otra variable, ‘velocidad’ especificaremos la rapidez de los movimientos.
Para conseguir controlar la velocidad de los movimientos se ha establecido este procedimiento:
//Modificará posición del servo mientras flag=1
while (flag){
flag=0;
flag=0;
//Si el eje brazo no está en su posición…
if (pwm_brazo != brazo){
if (pwm_brazo != brazo){
//…retrocede una posición si la nueva es menor
if (pwm_brazo > brazo) –pwm_brazo;
//…o avanza una posición si la nueva es mayor
else if (pwm_brazo < brazo) ++pwm_brazo;
else if (pwm_brazo < brazo) ++pwm_brazo;
//Con retardo de movimiento de posición indicado
delay_ms(velocidad);
//Y se comprueba otra vez si coincide posición actual
//con posición deseada
flag=1;
}
}
Y la forma de indicar las posiciones del servo mediante dos procedimientos. Por un lado indicando numéricamente las nuevas posiciones de los servos que han de cambiar:
velocidad=5; //Velocidad del movimiento
muneca= 16; //Nueva posición muñeca
brazo=20; //Nueva posición brazo
hombro=13; //Nueva posición hombro
brazo=20; //Nueva posición brazo
hombro=13; //Nueva posición hombro
//Acceso a la función del cambio de posiciones
movimiento_brazo_num (muneca, pinza, brazo, hombro, velocidad);
O tener unas posiciones preestablecidas definidas por una opción que se envía para obtener la nueva posición:
brazo_pos=2; //Brazo levantado
//Acceso a la función del cambio de posiciones
movimiento_brazo (brazo_pos,velocidad);
Donde brazo_pos indicará la posición genericamente preestablecida:
switch (brazo_pos){
//Brazo recogido
case0:muneca=8;pinza=8;brazo=5;hombro=21;break;
//Brazo recogido
case0:muneca=8;pinza=8;brazo=5;hombro=21;break;
//Brazo recogido
case1:muneca=pwm_muneca;pinza=pwm_pinza;brazo=5;hombro=21;break;
//Brazo levantado
case2:muneca=pwm_muneca;pinza=pwm_pinza;brazo=16;hombro=16;break;
|
|
|
//Brazo abajo extendido
case9:muneca=pwm_muneca;pinza=pwm_pinza;brazo=16;hombro=11;break;
}
La generación de las señales pulsatorias para el control de los servos se realiza mediante la interrupción por rebose del timer 0. Con cada rebose del timer 0 se accede a la función de interrupción donde se incrementa la variable “Ancho_pulso” y comparandola con cada una de las variables que contienen la posición de los servos se decide cuando la señal de control correspondiente a cada servo debe pasar a cero. Cuando la variable “Ancho_pulso” incrementandose pasa de 0xff a 0×00, comienza un nuevo ciclo y por tanto un nuevo pulso para todos los servos. De esta forma se consigue un pulso cíclico para los servos de entre unos 0,9 ms a 2,1 ms cuando establecemos valores de la variable del servo de entre unos 7 y 21, correspondientes a las posiciones extremas. ¡Ojo! Todos estos valores corresponden al uso de un cristal de cuarzo de 4 MHz y un preescaler de 32 del timer 0.
Codigo:
////////////////////////////////////////////////////////////////////////////////
// //
// BRAZO ROBOT //
// //
// (c) RobotyPic 2011 //
// //
////////////////////////////////////////////////////////////////////////////////
#include
#fuses NOWDT
#fuses XT //Oscilador por cristal entre 4Mhz y 10Mhz
#fuses NOBROWNOUT //No reset por baja tensión
#use delay(clock=4000000) //Frecuencia del cristal oscilador 4MHz
#byte trisa=0x85
#byte porta=0x05
#bit Bit_PWM_muneca = PORTA.0 //Bit 0 puerto A Salida modulación muñeca
#bit Bit_PWM_pinza = PORTA.1 //Bit 1 puerto A Salida modulación pinza
#bit Bit_PWM_brazo = PORTA.2 //Bit 2 puerto A Salida modulación codo
#bit Bit_PWM_hombro = PORTA.3 //Bit 3 puerto A Salida modulación hombro
/********************** Prototipos de las funciones ***************************/
void main (void); //función principal
void generacion_pwm (void); //genera señales moduladas control de servos
void movimiento_brazo (void); //Mueve brazo con retardo de movimientos
/********************** Variables para movimiento brazo ***********************/
int8 PWM_muneca=0,PWM_pinza=0,PWM_brazo=0,PWM_hombro=0; //Guardará los valores de las señales PWM
int8 Ancho_pulso=0;
short int flag;
int8 muneca=0, pinza=0, brazo=0, hombro=0;
int8 brazo_pos;
int8 velocidad=3; //Lentitud de los movimientos
/******************************************************************************/
/********* FUNCIÓN GENERACIÓN MODULACIONES PWM PARA SERVOS BRAZO **************/
#int_Timer0
void generacion_pwm() {
Ancho_pulso++; //Incremento cada rebose del timer0
if (Ancho_pulso==0) {
Bit_PWM_muneca =1;
Bit_PWM_pinza =1;
Bit_PWM_brazo =1;
Bit_PWM_hombro =1;
}
if (Ancho_pulso==PWM_pinza)
Bit_PWM_pinza=0;
if (Ancho_pulso==PWM_brazo)
Bit_PWM_brazo=0;
if (Ancho_pulso==PWM_hombro)
Bit_PWM_hombro=0;
if (Ancho_pulso==PWM_muneca)
Bit_PWM_muneca=0;
set_timer0(255);
}
/****************************************************************************/
/*********** FUNCIÓN MOVIMIENTO BRAZO POR ESTADOS PREESTABLECIDOS ***********/
void movimiento_brazo (brazo_pos, velocidad){
switch (brazo_pos){
//Brazo recogido
case 0: muneca=8; pinza=8; brazo=5; hombro=21; break;
//Brazo recogido
case 1: muneca=pwm_muneca; pinza=pwm_pinza; brazo=5; hombro=21; break;
//Brazo levantado
case 2: muneca=pwm_muneca; pinza=pwm_pinza; brazo=16; hombro=16; break;
//Brazo levantado extendido
case 3: muneca=pwm_muneca; pinza=pwm_pinza; brazo=20; hombro=11; break;
//Brazo semiextendido
case 4: muneca=pwm_muneca; pinza=pwm_pinza; brazo=5; hombro=16; break;
//Girar muñeca
case 5: muneca=16;pinza=pwm_pinza;brazo=pwm_brazo; hombro=pwm_hombro;break;
//Regirar muñeca
case 6: muneca=8; pinza=pwm_pinza;brazo=pwm_brazo; hombro=pwm_hombro;break;
//Abrir pinza
case 7: muneca=pwm_muneca;pinza=19;brazo=pwm_brazo; hombro=pwm_hombro;break;
//Cerrar pinza
case 8: muneca=pwm_muneca;pinza=8;brazo=pwm_brazo; hombro=pwm_hombro;break;
//Brazo abajo extendido
case 9: muneca=pwm_muneca; pinza=pwm_pinza; brazo=16; hombro=11; break;
}
flag=1; //Permiso para revisar posiciones del brazo
while (flag){
flag=0; //Cuando todos servos en posición se sale del while
if (pwm_muneca != muneca) { //Si muñeca no está en su posición...
if (pwm_muneca > muneca) --pwm_muneca;//...retrocede una posición...
else if (pwm_muneca < muneca) ++pwm_muneca; //...o avanza una posición delay_ms(velocidad); //Retardo mivimiento de posición flag=1; //Una posición avanzada } if (pwm_pinza != pinza) { //Si pinza no está en su posición... if (pwm_pinza > pinza) --pwm_pinza; //...retrocede una posición...
else if (pwm_pinza < pinza) ++pwm_pinza; //...o avanza una posición flag=1; //Una posición avanzada } if (pwm_hombro != hombro) { //Si hombro no está en su posición... if (pwm_hombro > hombro) --pwm_hombro; //...retrocede una posición...
else if (pwm_hombro < hombro) ++pwm_hombro; //...o avanza una posición delay_ms(velocidad); //Retardo mivimiento de posición flag=1; //Una posición avanzada } if (pwm_brazo != brazo) { //Si brazo no está en su posición... if (pwm_brazo > brazo) --pwm_brazo; //...retrocede una posición...
else if (pwm_brazo < brazo) ++pwm_brazo; //...o avanza una posición delay_ms(velocidad); //Retardo mivimiento de posición flag=1; //Una posición avanzada } } delay_ms(50); } /****************************************************************************/ /************* FUNCIÓN MOVIMIENTO BRAZO POR VALORES NUMÉRICOS ***************/ void movimiento_brazo_num (muneca, pinza, brazo, hombro, velocidad){ flag=1; //Permiso para revisar posiciones del brazo while (flag){ flag=0; //Cuando todos servos en posición se sale del while if (pwm_muneca != muneca) { //Si muñeca no está en su posición... if (pwm_muneca > muneca) --pwm_muneca; //...retrocede una posición...
else if (pwm_muneca < muneca) ++pwm_muneca;//...o avanza una posición delay_ms(velocidad); //Retardo mivimiento de posición flag=1; //Una posición avanzada } if (pwm_pinza != pinza) { //Si pinza no está en su posición... if (pwm_pinza > pinza) --pwm_pinza; //...retrocede una posición...
else if (pwm_pinza < pinza) ++pwm_pinza; //...o avanza una posición flag=1; //Una posición avanzada } if (pwm_hombro != hombro) { //Si hombro no está en su posición... if (pwm_hombro > hombro) --pwm_hombro; //...retrocede una posición...
else if (pwm_hombro < hombro) ++pwm_hombro; //...o avanza una posición delay_ms(velocidad); //Retardo mivimiento de posición flag=1; //Una posición avanzada } if (pwm_brazo != brazo) { //Si brazo no está en su posición... if (pwm_brazo > brazo) --pwm_brazo; //...retrocede una posición...
else if (pwm_brazo < brazo) ++pwm_brazo; //...o avanza una posición
delay_ms(velocidad); //Retardo mivimiento de posición
flag=1; //Una posición avanzada
}
}
delay_ms(50);
}
/******************************************************************************/
/*************************** FUNCIÓN PRINCIPAL ********************************/
void main(){
//INICIALIZACIÓN trisa=0x00; //Puerto A todo salidas
//Posición inicial del brazo
pwm_muneca=8; //muñeca recta
pwm_pinza=8; //pinza cerrada
pwm_brazo=5; //codo recogido
pwm_hombro=21; //hombro recogido
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_32); //Configuración generación PWM
enable_interrupts(INT_TIMER0); //Inhabilitación interrupción generación pwm
enable_interrupts (GLOBAL);
delay_ms(100); //Estabilización en el arranque del sistema
while (1){
//Movimiento del brazo con controles predefinidos
velocidad=3;
brazo_pos=1; //Brazo recogido
movimiento_brazo (brazo_pos,velocidad);
brazo_pos=2; //Brazo levantado
movimiento_brazo (brazo_pos,velocidad);
brazo_pos=3; //Brazo levantado extendido
movimiento_brazo (brazo_pos,velocidad);
brazo_pos=7; //Abrir pinza
movimiento_brazo (brazo_pos,velocidad);
brazo_pos=5; //Girar muñeca
movimiento_brazo (brazo_pos,velocidad);
brazo_pos=6; //Regirar muñeca
movimiento_brazo (brazo_pos,velocidad);
brazo_pos=5; //Girar muñeca
movimiento_brazo (brazo_pos,velocidad);
brazo_pos=6; //Regirar muñeca
movimiento_brazo (brazo_pos,velocidad);
delay_ms(300); //descanso en los movimientos
brazo_pos=8; //Cerrar pinza
movimiento_brazo (brazo_pos,velocidad);
brazo_pos=2; //Brazo levantado
movimiento_brazo (brazo_pos,velocidad);
brazo_pos=9; //Brazo recogido
movimiento_brazo (brazo_pos,velocidad);
delay_ms(300); //descanso en los movimientos
//Movimiento del brazo mediante controles numéricos
velocidad=6; //Movimiento más lento
muneca= 20; //Girar muñeca
movimiento_brazo_num (muneca, pinza, brazo, hombro, velocidad);
pinza=19; //Abrir pinza
movimiento_brazo_num (muneca, pinza, brazo, hombro, velocidad);
delay_ms(50);
pinza=8; //Cerrar pinza
movimiento_brazo_num (muneca, pinza, brazo, hombro, velocidad);
delay_ms(300); //descanso en los movimientos
velocidad=10; //Movimiento más rápido
brazo=20; //Extender brazo
movimiento_brazo_num (muneca, pinza, brazo, hombro, velocidad);
velocidad=15; //Movimiento más lento
muneca= 8; //Girar muñeca
movimiento_brazo_num (muneca, pinza, brazo, hombro, velocidad);
delay_ms(100); //descanso en los movimientos
velocidad=5; //Movimiento más lento
muneca= 16; //Girar muñeca
brazo=20; //Mover brazo
hombro=13; //Mover hombro
movimiento_brazo_num (muneca, pinza, brazo, hombro, velocidad);
delay_ms(300);
velocidad=3;
muneca= 8; //Girar muñeca
pinza=8; //Cerrar pinza
brazo=5; //Mover brazo
hombro=21; //Mover hombro
movimiento_brazo_num (muneca, pinza, brazo, hombro, velocidad);
}
}
También te puede interesar control de 8 servos:
|
|
 | 11 septiembre 2013 en Electronica | tags: Circuitos electrónicos, Electronica |
