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.
pulsa en la imagen para ampliarla
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;
//Si el eje brazo no está en su posición...
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;
//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
//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
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 0x00, 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.
[+/-] Ver / Ocultar programa completo en C
////////////////////////////////////////////////////////////////////////////////
// //
// BRAZO ROBOT //
// //
// (c) RobotyPic 2011 //
// //
////////////////////////////////////////////////////////////////////////////////
#include <16F876A.h>
#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);
}
}
Todos los archivos se pueden descargar en el siguiente enlace:
Descarga de archivos Brazo Robot
El proyecto contenido en el artículo ha sido diseñado con la versión v4.084 de CCS PIC C COMPILER
CÓDIGO DE COLORES Y NOMENCLATURA PARA CONDENSADORES
CÓDIGO DE COLORES PARA RESISTENCIAS
No hay comentarios:
Publicar un comentario