Robotica

126
CONCLUSIONES
La robtica es sin duda un rea de inters de las investigaciones de la

actualidad, con esta investigacin se estudio sobre los robots manipuladores
donde, se pudo apreciar un desarrollo completo desde la identificacin de
una necesidad, hasta llevar a cabo la construccin de un prototipo,
contemplando a grandes rasgos una solucin con una estructura mecnica
de brazo robtico de 5 grados de libertad, su estudio matemtico de
cinemtica directa e inversa para finalmente desarrollar un control cinemtico
que se encarga de los movimientos y control de trayectoria.
Para el cumplimiento del objetivo principal de la tesis, se determin que el
tipo de robot angular es el que mejor se adapta a las tareas de movilizacin,
de igual manera se propuso una clula de trabajo de robot en el centro para
que pudiese interactuar, tal cual como lo hace un operador de planta en la
actualidad, donde este toma una pieza, la coloca en el troquel, la procesa
para despus poner esa misma pieza en una canasta para que este sea
procesado
en
otras
actividades,
igualmente
se
analizo
las
reglas
matemticas para poder modelar el robot industrial y poder contemplar su

movimiento en el espacio cartesiano y en el espacio articular con el fin de
que este pueda realizar tareas especificas de movimiento con cierta
precisin.
Una vez realizado su estudio cinemtico se desarrollo el control
cinemtico a partir de la generacin de trayectorias con interpolaciones
127
cubicas entre puntos adyacentes para asegurar un movimiento suave sin

cambios bruscos, se implemento el control embebido en un microcontrolador
PIC18F4550, as mismo y para poderle suministrar las instrucciones al robot
industrial, se desarrollo un sistema de programacin del robot, el cual se
contruyo bajo el esquema de programacin textual, para el cual se defini
una serie de comandos que el operador podr usar para definir la orientacin
y trayectoria que debe seguir la pinza del robot.
Se cumpli con el objetivo principal de este estudio con xito ya que el
robot industrial funciona correctamente y es una herramienta de gran utilidad
ya que se puede adaptar fcilmente a las actividades de una empresa
manufacturera, partiendo de los comandos ingresados en el programa del
mismo, siendo este una alternativa tecnolgica al alcance de la pequea y
mediana empresa para automatizar el proceso de conformado de piezas en
prensas troqueladoras, incrementando la seguridad y productividad del
proceso.
128
RECOMENDACIONES
Segn la experiencia obtenida con el diseo del control cinemtico y su

implementacin en un microcontrolador PIC18F4550, se plantea la primera
recomendacin, que es utilizar un dspic o un microcontrolador de 24 o32 bits
para tener los suficientes recursos para implementar con mayor precisin y
rapidez las operaciones aritmticas y as agilizar el calculo de las mismas y a
la vez aumentar el rendimiento del control de trayectorias.
De igual manera al aumentar los recursos de procesamiento con un mejor
micro, tambin se recomienda que este posea mayor cantidad de memoria,
la cual se pueda usar para aumentar la cantidad de rdenes de un trabajo
que deba realizar el robot en actividades complejas.
Para implementaciones robustas en aplicaciones industriales como las
actividades de soldadura o corte guiado se recomienda usar adicionalmente
otra estrategia de control combinando un control dinmico sobre las
referencias que suministra el control cinemtico.
En cuanto al hardware se recomienda modificar los sensores y sus
codificadores, aumentando la resolucin de lo mismos, para optimizar la
precisin en los clculos y mejorar el rendimiento general del control.
129
REFERENCIAS BIBLIOGRAFICAS
Libros
Angeles, J., (2007). Fundamentals of Robotic Mechanical Systems, 3ra

Edicin, Editorial Springer, U.S.A.
Angulo, J., Romero S., Angulo, I., (2006). Microcontroladores PIC Diseo
Practico de Aplicaciones 2da Parte: PIC16F87X y PIC18FXXX, 2da
Edicin, Editorial McGRAW-HILL, Espaa.
Arias, F., (1999). El Proyecto de Investigacin, 2ra Edicin, Editorial
Episteme, Venezuela.
Axelson, J., (2005). USB Complete: Everything You Need to Develop
Custom USB Peripherals, 3ra Edicin, Lakeview Research LLC, U.S.A.
Barr, M., Massa, A., (2006), Programming Embedded Systems, 1ra Edicin,
Editorial OReilly. U.S.A.
Barrientos, A., Pein, L. F., Balaguer, C., Aracil, R., (2007), Fundamentos de
Robtica, 2da Edicin, Editorial McGraw-HILL. Espaa.
Craig, J., (1989), Introduction to Robotics: Mechanics and Control, 2da
Edicin, Editorial Addison-Wesley Publishing Company, Inc. U.S.A.
Fu, K., Gonzlez, R., Lee, C., (1987), Robtica: Control, sensores, visin e
inteligencia, 2da Edicin, Editorial McGraw-HILL. Mxico.
Groove, M. P., Weiss, M. R., Odrey, N., (1989), Robtica Industrial:
Tecnologa, Programacin y Aplicaciones, 1ra Edicin, Editorial
McGraw-HILL. Espaa.
130
Kelly, R., Santibez, V., (2003), Control de Movimiento de Robots

Manipuladores, 1ra Edicin, Editorial Prentice Hall. Espaa.
Lewis, P, H., Yang, S., (1999), Sistemas de Control en Ingeniera, 1ra
Edicin, Editorial Prentice Hall. Espaa.
Ogata, K., (1998), Ingeniera de Control Moderna, 3ra Edicin, Editorial
Prentice Hall, Mxico.
Ollero, A., (2007). Robtica: Manipuladores y Robots Mviles, 1ra Edicin,
Editorial Marcombo-Alfaomega, Mxico.
Renteria, A., Rivas M., (2000). Robtica Industrial, Fundamentos y
Aplicaciones, 1ra Edicin, Editorial McGRAW-HILL, Espaa.
Sampieri, R., Collado, C., (1991). Metodologa de la Investigacin, 1ra
Edicin, Editorial McGRAW-HILL, Mxico.
Torres, F., Pomares, J., Gil, P., Puente, S. T., Aracil, R., (2002), Robots y
Sistemas Sensoriales, 2da Edicin, Editorial Prentice Hall. Espaa.
Trabajos de Investigacin
Campa, R., (2005). Control de Robots Manipuladores en Espacio de Tarea,

Tesis doctoral realizada en el Centro de Investigacin Cientfica y de
Educacin Superior de Ensenada, Baja California, Mxico.
Jaramillo, F., (1999), Plataforma para el ensayo de algoritmos de control
para un robot manipulador, Tesis de maestra realizada en el Centro de
131
Investigacin Cientfica y de Educacin Superior de Ensenada, Baja

California, Mxico.
Marquez,
L.,
(1994).
Control
de
Robots
Manipuladores
Utilizando
Retroalimentacin Visual, Tesis de maestra realizada en el Centro de

Investigacin Cientfica y de Educacin Superior de Ensenada, Baja
California, Mxico.
Moreno, E., (1999), Control de Robots Manipuladores Mediante Campo de
Velocidad, Tesis de maestra realizada en el Centro de Investigacin
Cientfica y de Educacin Superior de Ensenada, Baja California,
Mxico.
Olcay, P., (2010), Sistema De Control de un Manipulador para un ROV
(Remotely
Operated
Vehicle),
Tesis
de
maestra
del
Instituto
Panamericano de Ingeniera Naval, Chile.

Sirit, Y., Fernandez, J., Lubo, A.,
(2002). Accidentes de la Mano en
Trabajadores de la Costa Oriental del Lago de Maracaibo del Estado

Zulia, Venezuela 1986-1993, Investigacin clnica, vol.43, no.2, de la
Universidad del Zulia, Venezuela.
Sobrado, E., (2003). Sistema De Visin Artificial Para El Reconocimiento Y
Manipulacin De Objetos Utilizando Un Brazo Robot, Tesis de maestra
desarrollada en la Pontificia Universidad Catlica del Per, Per.
ANEXOS A
rea de Troqueles de Gurimetal C.A
rea de Prensas Troqueladoras de Gurimetal C.A
Prensas Troqueladoras de Gurimetal C.A
ANEXOS B
Piezas en SolidWorks del Ensamble del Robot Industrial
Base del Robot Industrial y Electrnica
Detalle del Brazo del Robot Industrial y Pinza
Detalle de la Pinza del Robot Industrial
Vista Superior del Robot Industrial
Vista Lateral del Robot Industrial
Vista Transversal del Robot Industrial
ANEXOS C
Especificaciones Tcnicas de los Servomotores Hitec
ANEXOS D
Especificaciones Tcnicas de las Piezas Electrnicas
Conector USB tipo A
Disposicin pines conector USB tipo A

donde:
1=Vbus
2=D3=D+
4=Gnd
ANEXOS E
Cdigo del Programa de Control
Fichero pic_brazo_robot.c
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////
pic_brazo_robot
////
////
////
////
////
//// Realizado con el compilador CCS PCWH 4.023
////
////
////
//// Por: Ing. Omar Len leon.omar82@gmail.com
////
////
////
//// Robot Industrial para la Automatizacin del porceso de conformado ////
//// de piezas en Prensas Troqueladoras
////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include <18F4550.h>
#DEVICE ADC=10 // cad a 10 bits, justificacin a a la derecha
#fuses HSPLL,NOWDT,NOPROTECT,NOLVP,NODEBUG,USBDIV,PLL1,CPUDIV1,VREGEN
#use delay(clock=48000000) //configuramos la velocidad del micro a 48 MHz
#use fast_io(B)
#define USB_HID_DEVICE FALSE
//deshabilitamos el uso de las directivas HID
#define USB_EP1_TX_ENABLE USB_ENABLE_BULK //turn on EP1(EndPoint1) for IN bulk/interrupt transfers
#define USB_EP1_RX_ENABLE USB_ENABLE_BULK //turn on EP1(EndPoint1) for OUT bulk/interrupt transfers
#define USB_EP1_TX_SIZE 10
instruccion
#define USB_EP1_RX_SIZE 20
instruccion y 6 para velocidad
//tamano en bytes que envia al programa 2 por cada sensor y 2 para

//tamano en bytes que recibe del programa 2 por cada servo, 2 para
//solo vamos a configurar 8 servomotores digitales como maximo para el brazo robotico
//en realidad solo se usaran 2, que son los large servo del hombro y codo del brazo
#define SERVO1 PIN_B0
#define lineas_programa 20
#priority timer1
const int16 ra = 133;
const int16 Ticks4Window = 30012; // PWM Window for servo = 2.5 ms x 8 = 20 ms 2.5/0.0000833=30012.00
const int16 Ticks4Minimum = 10800; // PWM High for Minimum Position = 0.9 ms 0.9/0.0000833=10804.32
const int16 Ticks4Center = 18000; // PWM High for Center Position = 1.5 ms
1.5/0.0000833=18007.20
const int16 Ticks4Maximum = 25210; // PWM High for Maximum Position = 2.1 ms 2.1/0.0000833=25210.08
//const int16 TicksTotal = 65535;
const int16 TicksTotal = 65640; // se modifico para poner exactos 20 ms
//configuramos los tiempos iniciales para cada uno de los servos a utilizar
//valores calculados con el software de configuracion robotico v1.0
//para este caso tenemos una precision muy alta de 0.0075 grados de resolucion aprox
//establecemos por defecto un punto de reset
const int16 Ticks4Servo01 = 18000;
//establesco el equivalente a 1.5 ms punto medio de los servos, por estandarizacion solo
//estos son unsigned long
int16 Ticks4Servo01_medio = 18000;

//variables con los limites superiores e inferiores de los servos, se colocan por defecto
int16 Ticks4Servo01_sup = 25500;
int16 Ticks4Servo01_inf = 7000;
static int16
Servo_PWM[8]={Ticks4Servo01,Ticks4Servo02,Ticks4Servo03,Ticks4Servo04,Ticks4Servo05,Ticks4Servo06,Ticks
4Center,Ticks4Center};
static int8 Servo_Idx=0;
static int1 SERVO1_ON=1;
static int1 flag_Phase; //indica si esta la senal en 0 o en 1
static int16 Ticks4NextInterrupt=35536; //a 48Mhz segun calculadora de RedRaven
#int_timer1
void timer1_isr(void)
{
if(flag_Phase==0)
{
if(Servo_Idx==0 && SERVO1_ON)
output_high(SERVO1);
Ticks4NextInterrupt = TicksTotal - Servo_PWM[Servo_Idx];
set_timer1(Ticks4NextInterrupt);
}
if(flag_Phase==1)
{
output_low(SERVO1);
output_low(SERVO2);
output_low(SERVO3);
output_low(SERVO4);
output_low(SERVO5);
output_low(SERVO6);
output_low(SERVO7);
output_low(SERVO8);
Ticks4NextInterrupt = TicksTotal - Ticks4Window + Servo_PWM[Servo_Idx];
if(++Servo_Idx>7) Servo_Idx=0;
}
++flag_Phase;
}
int8 i=0, _n=0, k=0; //Indice para la matriz de programa y _n para la cantidad de lineas
signed long programa[lineas_programa][7]; //20 lineas de programa maximo y 7 bytes para cada instruccion (uno
para cada punto x y z, angulo, apertura de pinza y uno para la velocidad minima)
// o 20 lineas con 7 bytes para cada una de sus articulaciones
signed long q[lineas_programa][6];
//q lo va a generar el control cinematico inverso; o directamente la
cinematica directa
//q no puede ser negativo hay se guardan los angulos aunue si se cambia causa error
//al calcular los coeficientes a,b,c,d
float qd[lineas_programa][4];
//qd almacena las velocidades de paso para las primeras 4 articulaciones
unsigned long _q1, _q2, _q3, _q4, _q5, _q6;
//no pueden ser negativos
/////////////////////////////////////////////////////////////////////////////
//
// Se incluyen las librerias propias de CCS..
//
/////////////////////////////////////////////////////////////////////////////
#include <pic18_usb.h>
//Capa Microchip PIC18Fxx5x Hardware para el driver USB de los pics
#include <PicUSB.h>
//Configuracin del USB y los descriptores para este dispositivo
#include <usb.c>
//handles usb setup tokens and get descriptor reports
#include <pic_brazo_control.h> //Definicion de la cinematica del manipulador
#define menu01
#define menu02
#define servo01_1
#define servo01_2
#define servo02_1
#define servo02_2
#define servo03_1
#define servo03_2
#define servo04_1
#define servo04_2
#define servo05_1
#define servo05_2
#define servo06_1
#define servo06_2
#define velocidad01
#define velocidad02
#define velocidad03
#define velocidad04
#define velocidad05
#define velocidad06
recibe[0] //definimos nombres para manejar mejor el arreglo de recibo

recibe[1]
recibe[2]
recibe[3]
recibe[4]
recibe[5]
recibe[6]
recibe[7]
recibe[8]
recibe[9]
recibe[10]
recibe[11]
recibe[12]
recibe[13]
recibe[14]
recibe[15]
recibe[16]
recibe[17]
recibe[18]
recibe[19]
#define resultado01 envia[0] //definimos nombre para manejar mejor el envio
#define resultado02
#define sensor01_1
#define sensor01_2
#define sensor02_1
#define sensor02_2
#define sensor03_1
#define sensor03_2
#define sensor04_1
#define sensor04_2
envia[1]
envia[2]
envia[3]
envia[4]
envia[5]
envia[6]
envia[7]
envia[8]
envia[9]
void reset()
{
Servo_PWM[0] = Ticks4Servo01;
delay_ms(200);
}
void punto_reset(int p)
{
programa[p][0] = 135; //punto q1
programa[p][6] = 3; //delay
}
void main(void) {
long vref_value;
//lee valor analgico
int parte_baja;
//para separar el byte bajo despus de leer valor analgico
int parte_alta;
//para separar el byte alto despus de leer valor analgico
int ADC_ACQT_2TAD=0x1;
int8 recibe[20];
int8 envia[10];
//declaramos variables
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);
setup_adc_ports(AN0_TO_AN8 || VSS_VDD);
setup_adc(ADC_CLOCK_DIV_64 || ADC_ACQT_2TAD );
set_tris_a(0b11110100);
set_tris_e(0b11100000);
set_tris_b(0b00000000);
set_tris_c(0b00000000);
set_tris_d(0b00000000);
output_low(SERVO1);
output_low(SERVO2);
output_low(SERVO3);
output_low(SERVO4);
output_low(SERVO5);
output_low(SERVO6);
//AN0_TO_AN4
//AN5_TO_AN7
output_low(SERVO7);
output_low(SERVO8);
enable_interrupts(int_timer1);
enable_interrupts(global);
usb_init();
//inicializamos el USB
usb_task();
//habilita perifrico usb e interrupciones
//hay que colocar en comentario esta lnea si se desea simular con MPLAB
usb_wait_for_enumeration();
//esperamos hasta que el PicUSB sea configurado por el host
while (TRUE)
{
if(usb_enumerated())
//si el PicUSB est configurado
{
if (usb_kbhit(1))
//si el endpoint de salida contiene datos del host
{
usb_get_packet(1, recibe, 20); //cogemos el paquete de tamao 20 bytes del EP1 y almacenamos en recibe
//usb_get_packet(endpoint, *ptr, max)
//el argumento 1: endpoint: es el buffer de datos de entrada, donde llega el paquete de datos.
//el argumento 2 *prt: es el apuntador adonde guardaremos ese dato.
//el argumento 3 max: es el tamao de paquete en bytes.
if (menu01 == 1) // Modo Prueba
{
//Aqui se carga el vector de pulsos de los servos
Servo_PWM[0]=make16(servo01_2,servo01_1); //Base
Servo_PWM[1]=make16(servo02_2,servo02_1); //Hombro
Servo_PWM[2]=make16(servo03_2,servo03_1); //Codo
Servo_PWM[3]=make16(servo04_2,servo04_1); //muneca
Servo_PWM[4]=make16(servo05_2,servo05_1); //muneca
Servo_PWM[5]=make16(servo06_2,servo06_1); //dedos
}
if (menu01 == 2) // Modo Reset
{
reset();
}
if (menu01 == 3) // Inicio de Grabacin
{
//colocamos el ndice de la matriz de programa en 1 y colocamos el primer
//punto de la interpolacin a 0
punto_reset(0);
i=1; _n=1;
}
if (menu01 == 4) // Finaliza la Grabacin
{
//La cantidad es igual al indice
punto_reset(i);
i++;
_n=i;
}
if (menu01 == 5) // Se inicia el programa guardado // si es 5.1 cinemtica inversa si es 5.2 cinemtica directa
{
if (menu02 == 1) //cinemtica inversa
{
for(k=0;k<_n;k++)
{
cinematica_inversa(programa[k][0],programa[k][1],programa[k][2],programa[k][3],programa[k][4],k);
//X
Y
Z
Angulo
Apertura
}
}
if (menu02 == 2) //cinemtica directa
{
for(k=0;k<_n;k++)
{
cinematica_directa(programa[k][0],programa[k][1],programa[k][2],programa[k][3],programa[k][4],programa[k][5],k);
//q1
q2
q3
q4
q5
q6
}
}
interpolacion_cubica(_n,menu02);
reset();
}
if (menu01 == 6) // Se finaliza el programa guardado
{
reset();
}
if (menu01 == 8) // Modo Punto
{
//guardamos el paso del programa
if(i<lineas_programa)
{
if (menu02 == 1) // Modo Punto XYZ
{
programa[i][0] = make16(servo02_1,servo01_2); //punto X
if(servo01_1) programa[i][0] = programa[i][0] * -1;
programa[i][1] = make16(servo03_2,servo03_1); //punto Y
programa[i][2] = make16(servo05_1,servo04_2); //punto Z //No puede ser negativo
programa[i][3] = make16(servo06_2,servo06_1); //angulo, normalmente en 0
programa[i][4] = make16(0,velocidad01); //Apertura pinza, de 0 a 100
programa[i][5] = make16(0,velocidad02); //velocidad, de 0 a 100
programa[i][6] = make16(0,velocidad03); //delay
}
if (menu02 == 2) // Modo Punto ANG
{
programa[i][0] = make16(servo01_2,servo01_1); //punto q1
programa[i][6] = make16(0,velocidad01); //delay
}
i++;
}
}
if (menu01 == 9) // Modo Prueba cinemtica
{
programa[0][0] = make16(servo02_1,servo01_2); //punto X
programa[0][1] = make16(servo03_2,servo03_1); //punto Y
programa[0][2] = make16(servo05_1,servo04_2); //punto Z //No puede ser negativo
programa[0][3] = make16(servo06_2,servo06_1); //ngulo, normalmente en 0

programa[0][4] = make16(0,velocidad01); //Apertura pinza, de 0 a 100
programa[0][5] = make16(0,velocidad02); //velocidad, de 0 a 100
}
}
}//
}
}
Fichero pic_brazo_robot.h
#include<math.h>
//Basado en los parmetros de Denavit-Hartenberg se generan las
//siguientes constantes
#define ALTURA_BASE 70.00
//Altura de la Base
#define BRAZO
198.00 //Hombro-a-Codo
#define ANTEBRAZO 244.00 //Codo-a-Mueca
#define MUNECA
98.00
//Mueca - incluyendo la extensin del ultimo engranaje
#define ftl(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) //conversin de float a long
/*Variables pre calculadas */
float brazo_sq = BRAZO*BRAZO;
float antebrazo_sq = ANTEBRAZO*ANTEBRAZO;
float radianes(float grados)
{
return (grados*(pi/180));
}
float grados(float radianes)
{
return (radianes*(180/pi));
}
int signo(float s)
{
if(s>0)
return 1;
if(s==0)
return 0;
else
return -1;
}
void posicion_servos(int16 q1, int16 q2, int16 q3, int16 q4, int16 q5, int16 q6)
{
//Base
if(q1 > Ticks4Servo01_sup)
q1 = Ticks4Servo01_sup;
if(q1 < Ticks4Servo01_inf)
q1 = Ticks4Servo01_inf;
Servo_PWM[0] = q1;
//Hombro
Servo_PWM[1] = q2;
//Codo
Servo_PWM[2] = q3;
//muneca
Servo_PWM[3] = q4;
//muneca
Servo_PWM[4] = q5;
//dedos
Servo_PWM[5] = q6;
}
void cinematica_directa(signed long q1, signed long q2, signed long q3, signed long q4, signed long q5, signed long
q6, int paso)
{
q[paso][0] = q1;
q[paso][1] = q2;
q[paso][2] = q3;
q[paso][3] = q4;
q[paso][4] = q5;
q[paso][5] = q6;
}
/* rutina para el posicionamiento utilizando la cinemtica inversa */
/* (z) es altura, por la tanto no puede ser negativa, (y) es la distancia desde el centro de la base */
void cinematica_inversa(float x, float y, float z, float angulo_muneca_d, int apert_pinza, int paso)
{
float angulo_muneca_r;
float base_angulo_r;
float angulo_mano_d;
float rdist;
float muneca_dist_z;
float muneca_dist_y;
float muneca_z;
float muneca_y;
float s_w;
float s_w_sqrt;
float a1;
float a2;
float angulo_hombro_r;
float angulo_hombro_d;
float angulo_codo_r;
float angulo_codo_d;
float angulo_codo_dn;
float q1;
float q2;
float q3;
float q4;
angulo_muneca_r = radianes( angulo_muneca_d ); //angulo en radianes para los calculos

//formula 28 del documento hallamos q1 (ngulo base)
base_angulo_r = atan2( x, y );
//formula 29 del documento hallamos (distancia desde x,y)
rdist = sqrt(( x * x ) + ( y * y ));
/* rdist es la coordenada y para el manipulador */
y = rdist;
muneca_dist_z = ( sin( angulo_muneca_r )) * MUNECA;
muneca_dist_y = ( cos( angulo_muneca_r )) * MUNECA;
/* Se calcula la posicion de la muneca */
muneca_z = ( z - muneca_dist_z ) - ALTURA_BASE;
muneca_y = y - muneca_dist_y;
s_w = ( muneca_z * muneca_z ) + ( muneca_y * muneca_y );
s_w_sqrt = sqrt( s_w );
//formula 30 del documento
a1 = atan2( muneca_z, muneca_y );
a2 = acos((( brazo_sq - antebrazo_sq ) + s_w ) / ( 2 * BRAZO * s_w_sqrt ));
angulo_hombro_r = a1 + a2;
angulo_hombro_d = grados( angulo_hombro_r );
/* angulo del hombro */
angulo_codo_r = acos(( brazo_sq + antebrazo_sq - s_w ) / ( 2 * BRAZO * ANTEBRAZO ));
angulo_codo_d = grados( angulo_codo_r );
angulo_codo_dn = -( 180.0 - angulo_codo_d );
/* angulo de la muneca */
angulo_mano_d = ( angulo_muneca_d - angulo_codo_dn ) - angulo_hombro_d;
q1 = grados( base_angulo_r ); //validado
q2 = angulo_hombro_d - 90.0; //por validar
q3 = angulo_codo_d - 90.0;
//validado
q4 = angulo_mano_d;
q[paso][0] = ftl(q1);
}
//modo 1:cinematica inversa 2:c inematica directa
void interpolacion_cubica(int8 n, int8 modo)
{
//los puntos estan en la matriz programa
int8 npuntos = 10;
//numeros de puntos a generar por intervalo
int8 i_aux,s;
float _i;
float a[4],b[4],c[4],d[4];
//aqui se almacenan los coeficientes para los 4 servos
float qt[4];
//una vez este lleno esste vector se porcede a establecer las senales de los servos
float t_aux, c1_aux, c2_aux, d1_aux, d2_aux;
int16 d0_aux;
int8 T = 1;
int16 aux;
int8 condicion_01, condicion_02;
signed int16 condicion_03, condicion_04;
//se obbtienen los coeficientes de los splines cubicos
//[ti,tf,a,b,c,d] para cada intervalo
//se usa t continuo en 1 para cada paso

//for para los primeros 4 servos
for(s=0;s<4;s++)
{
qd[0][s] = 0; //el primero es cero
qd[n-1][s] = 0; //el ultimo es cero
for(i_aux=1;i_aux<n-1;i_aux++)
{
condicion_01 = signo(q[i_aux][s]-q[i_aux -1][s]);
condicion_02 = signo(q[i_aux+1][s]- q[i_aux][s]);
if(((condicion_01)==(condicion_02))||(q[i_aux][s]==q[i_aux+1][s])||(q[i_aux-1][s]==q[i_aux][s]))
{
condicion_03 = q[i_aux+1][s]-q[i_aux][s];
condicion_04 = q[i_aux][s]-q[i_aux -1][s];
//ejecutamos la formula por partes
//qd[i_aux][s] = 0.5 * ( ((condicion_03)/((i_aux+1)-i_aux)) + ((condicion_04)/(i_aux -(i_aux -1))));
c1_aux = (condicion_03)/((i_aux+1)-i_aux);
c2_aux = (condicion_04)/(i_aux -(i_aux-1));
t_aux = c1_aux + c2_aux;
qd[i_aux][s] = 0.5 * t_aux;
}
else
qd[i_aux][s] = 0;
}
}
//hasta aqu se tiene
//q[][servo]
q tiene los ngulos para cada lnea de instruccin
//qd[][servo] qd define las velocidades de paso las cuales se calcularon a partir de la ecuacin 6.6 pag.290
//(Fundamentos de Robtica)
//se debe obtener ahora los coeficientes A, B, C y D para el polinomio cubico segn pag.300 (Robots y Sistemas
Sensoriales)
//por lo que se va a obtener A,B,C,D del primer servo para la primera instruccion
T = 1;
for(i_aux=0;i_aux<n-1;i_aux++)//la posicion tambien da el tiempo
{
for(s=0;s<4;s++)
{
a[s] = q[i_aux][s];
b[s] = qd[i_aux][s];
c1_aux = (3/T*T*(q[i_aux+1][s]-q[i_aux][s]));
c2_aux = ((1/T) *(qd[i_aux+1][s]+(2*qd[i_aux][s])));
//c[s] = (3/T*T*(q[i_aux+1][s]- q[i_aux][s])) - ((1/T) *(qd[i_aux+1][s]+(2*qd[i_aux][s])));
c[s] = c1_aux - c2_aux;
d0_aux = q[i_aux+1][s]- q[i_aux][s];
//6 0
//d1_aux = (-2/(T*T*T))*(d0_aux);
d1_aux = (2*d0_aux)*-1;
d2_aux = (1/(T*T))*(qd[i_aux+1][s]+qd[i_aux][s]);
//d[s] = (-2/(T*T*T))*(q[i_aux+1][s]-q[i_aux][s]) + (1/(T*T))*(qd[i_aux+1][s]+qd[i_aux][s]);
d[s] = d1_aux + d2_aux;
}
//ya tenemos los coeficientes para cada servo (qt(t))
//ahora cuadramos las senales con mas resolucion en tiempo en este caso 10 puntos entre tramo
//aqui se envia el punto y luego la interpolacin
//tomamos prestada la variable c1_aux para no crear otra nueva
if(modo == 1) //aplicamos la interpolacion basado en lo calculado con la cinematica inversa
{
aux = q[i_aux][0] * ra;
_q1 = Ticks4Servo01_medio - aux;
_q2 = Ticks4Servo02_medio + aux;

_q5 = Ticks4Servo05_medio;
}
if(modo == 2) //cinemtica directa
{
_q1 = (unsigned long)q[i_aux][0];
}
//posicion_servos(_q1,_q2,_q3,_q4,_q5,_q6);
//delay_ms(100);
for(_i=0.0;_i<1;_i=_i+0.01)
{
qt[0]=a[0]+b[0]*(_i)+c[0]*(_i)*(_i)+d[0]*(_i)*(_i)*(_i);
qt[1]=a[1]+b[1]*(_i)+c[1]*(_i)*(_i)+d[1]*(_i)*(_i)*(_i);
qt[2]=a[2]+b[2]*(_i)+c[2]*(_i)*(_i)+d[2]*(_i)*(_i)*(_i);
qt[3]=a[3]+b[3]*(_i)+c[3]*(_i)*(_i)+d[3]*(_i)*(_i)*(_i);
if(modo == 1) //aplicamos la interpolacin basado en lo calculado con la cinemtica inversa
{
//enviamos la senal a los servos
aux = qt[0] * ra;
aux = qt[1] * ra;
aux = qt[2] * ra;
aux = qt[3] * ra;
}
if(modo == 2) //cinemtica directa
{
_q1 = ftl(qt[0]*ra);
_q5 = q[i_aux][4]*ra;
_q6 = q[i_aux][5]*ra;
}
posicion_servos(_q1,_q2,_q3,_q4,_q5,_q6);
//sin este delay el robot no se mueve ya que los clculos se realizan muy rpido
//delay_us no funciona
delay_ms(programa[i_aux][6]);
//delay_ms(3);
}
}
}
ANEXOS F
Cdigo del Sistema de Programacin del Robot
Fichero frm_configura_movimiento.cs
using System;
using System.IO;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.Runtime.InteropServices;
using System.Xml;
using lib;
namespace RiCOS //Robotic Interface Configuration Software
{
public partial class frm_configura_movimiento : Form
{
//static pieza_robot base_robot;
PicUSBAPI _PicUSBAPI = new PicUSBAPI();
byte[] buf = new Byte[2];
uint[] ini_pos = new uint[6];
uint[] min_pos = new uint[6];
uint[] max_pos = new uint[6];
uint[] habilitado = new uint[6];
uint[] actual_pos = new uint[6];
public frm_configura_movimiento()
{
InitializeComponent();
}
private bool isDirty = new bool();
private bool isLoaded = new bool();
private string Filename = string.Empty;
private void frm_configura_movimiento_Load(object sender, EventArgs e)
{
int i = 0;
XmlDocument conf_actuadores = new XmlDocument();
conf_actuadores.Load("conf_actuadores.xml");
XmlNode nodo_actuador = conf_actuadores.DocumentElement;
foreach (XmlNode nodo_hijo in nodo_actuador.ChildNodes)
{
foreach (XmlNode nodo in nodo_hijo.ChildNodes)
{
if (nodo.Name == "habilitado")
habilitado[i] = uint.Parse(nodo.InnerText.ToString());
if (nodo.Name == "max_pos")
max_pos[i] = uint.Parse(nodo.InnerText.ToString());
if (nodo.Name == "min_pos")
min_pos[i] = uint.Parse(nodo.InnerText.ToString());
if (nodo.Name == "ini_pos")
{
ini_pos[i] = uint.Parse(nodo.InnerText.ToString());
actual_pos[i] = uint.Parse(nodo.InnerText.ToString());
}
}
i++;
}
}
private void btn_cancelar_Click(object sender, EventArgs e)
{
this.Close();
}
private void btn_guardar_Click(object sender, EventArgs e)
{
this.openFileDialog.DefaultExt = "*.rps";
this.openFileDialog.Filter = "Archivos RPS|*.rps";
if (this.openFileDialog.ShowDialog() == DialogResult.OK)
{
this.rtbText.LoadFile(this.openFileDialog.FileName, RichTextBoxStreamType.PlainText);
this.tbPath.Text = this.openFileDialog.FileName;
this.Filename = this.openFileDialog.SafeFileName;
this.isLoaded = true;
}
}
private void copiarToolStripMenuItem_Click(object sender, EventArgs e)
{
this.rtbText.Copy();
}
private void pegarToolStripMenuItem_Click(object sender, EventArgs e)
{
this.rtbText.Paste();
}
private void cortarToolStripMenuItem_Click(object sender, EventArgs e)
{
this.rtbText.Cut();
}
private void menu_Opening(object sender, CancelEventArgs e)
{
if (string.IsNullOrEmpty(this.rtbText.SelectedText))
{
this.menu.Items[0].Enabled = false;
this.menu.Items[1].Enabled = false;
}
else
{
this.menu.Items[0].Enabled = true;
this.menu.Items[1].Enabled = true;
}
}
private void rtbText_TextChanged(object sender, EventArgs e)
{
this.isDirty = true;
}
public void guardar()
{
SaveFileDialog saveFile = new SaveFileDialog();
saveFile.DefaultExt = "*.rps";
saveFile.Filter = "Archivos RPS|*.rps";
if (saveFile.ShowDialog() == System.Windows.Forms.DialogResult.OK && saveFile.FileName.Length > 0)
{
rtbText.SaveFile(saveFile.FileName, RichTextBoxStreamType.PlainText);
MessageBox.Show("Se guardo exitosamente " + System.IO.Path.GetFileName(saveFile.FileName),
"Aviso");
}
this.tbPath.Text = saveFile.FileName;
this.Filename = System.IO.Path.GetFileName(saveFile.FileName);
this.isLoaded = true;
this.isDirty = false;
}
private void limpiar()
{
tbPath.Text = string.Empty;
rtbText.Text = string.Empty;
this.isDirty = false;
this.isLoaded = false;
this.Filename = string.Empty;
}
private void btn_nuevo_Click(object sender, EventArgs e)
{
DialogResult result = DialogResult.Cancel;
if (isLoaded)
{
if (isDirty)
{
if ((result = MessageBox.Show("Desea guardar los cambios realizados a " + Filename + "?", "Aviso",
MessageBoxButtons.YesNoCancel)) == DialogResult.Yes)
{
rtbText.SaveFile(tbPath.Text, RichTextBoxStreamType.PlainText);
MessageBox.Show("Se guardo exitosamente " + System.IO.Path.GetFileName(tbPath.Text),
"Aviso");
limpiar();
}
else if (result == DialogResult.No)
limpiar();
else
return;
}
else
limpiar();
}
else
{
if (isDirty)
{
if ((result = MessageBox.Show("Desea guardar los cambios realizados al programa sin nombre?",
"Aviso", MessageBoxButtons.YesNoCancel)) == DialogResult.Yes)
{
guardar();
limpiar();
}
else if (result == DialogResult.No)
limpiar();
else
return;
}
else
limpiar();
}
}
private void btn_guardar_programa_Click(object sender, EventArgs e)
{
DialogResult result = DialogResult.Cancel;
if (isLoaded)
{
if (isDirty)
{
rtbText.SaveFile(tbPath.Text, RichTextBoxStreamType.PlainText);
MessageBox.Show("Se guardo exitosamente " + System.IO.Path.GetFileName(tbPath.Text), "Aviso");
}
}
else
{
if (isDirty)
{
if ((result = MessageBox.Show("Desea guardar los cambios realizados al programa sin nombre?",
"Aviso", MessageBoxButtons.YesNoCancel)) == DialogResult.Yes)
{
guardar();
}
else
return;
}
else
return;
}
}
private void btn_play_Click(object sender, EventArgs e)
{
//Se toma cada instruccion y se transforma a la forma que entiende el micro
//se recorre cada linea
foreach (String linea in rtbText.Lines)
{
if (linea.StartsWith("origen"))
_PicUSBAPI.Envia_Comando(2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
else if (linea.StartsWith("ini_grabar"))
else if (linea.StartsWith("fin_grabar"))
else if (linea.StartsWith("ini_trabajo"))
//se dejo fijo para cinematica directa
else if (linea.StartsWith("fin_trabajo"))
else if (linea.StartsWith("limites_sup"))
{
byte[] max_pos_servo01 = new Byte[2];
max_pos_servo01 = System.BitConverter.GetBytes(max_pos[0] * Program._ra);
_PicUSBAPI.Envia_Comando_Configuracion(7, 1, uint.Parse(max_pos_servo01[0].ToString()),
uint.Parse(max_pos_servo01[1].ToString()), uint.Parse(max_pos_servo02[0].ToString()),
uint.Parse(max_pos_servo06[1].ToString()), 0, 0, 0, 0, 0, 0);
}
else if (linea.StartsWith("limites_inf"))
{
byte[] min_pos_servo01 = new Byte[2];
min_pos_servo01 = System.BitConverter.GetBytes(min_pos[0] * Program._ra);
_PicUSBAPI.Envia_Comando_Configuracion(7, 1, uint.Parse(min_pos_servo01[0].ToString()),
uint.Parse(min_pos_servo01[1].ToString()), uint.Parse(min_pos_servo02[0].ToString()),
uint.Parse(min_pos_servo06[1].ToString()), 0, 0, 0, 0, 0, 0);
}
else if (linea.StartsWith("paso_xyz") || linea.StartsWith("prueba"))
{
//separamos x,y,z - el angulo se implementa en la muneca.
//linea.Remove(0, 5); //elimina la palabra
//linea.Replace("paso", "");
string[] XYZ = linea.Split(' ');
uint x_signo = 0; //positivo
uint y_signo = 0;
uint z_signo = 0;
byte[] buf_x = new Byte[2];
byte[] buf_y = new Byte[2];
byte[] buf_z = new Byte[2];
byte[] grip = new Byte[2];
if (XYZ[1].StartsWith("-")) { x_signo = 1; XYZ[1] = XYZ[1].Replace("-",""); }
if (XYZ[2].StartsWith("-")) { y_signo = 1; XYZ[2] = XYZ[2].Replace("-", ""); }
if (XYZ[3].StartsWith("-")) { z_signo = 1; XYZ[3] = XYZ[3].Replace("-", ""); }
buf_x = System.BitConverter.GetBytes(uint.Parse(XYZ[1].ToString()));
buf_y = System.BitConverter.GetBytes(uint.Parse(XYZ[2].ToString()));
buf_z = System.BitConverter.GetBytes(uint.Parse(XYZ[3].ToString()));
grip = System.BitConverter.GetBytes(uint.Parse(XYZ[4].ToString()));
if (linea.StartsWith("paso"))
_PicUSBAPI.Envia_Comando(8, 1, x_signo, uint.Parse(buf_x[0].ToString()),
uint.Parse(buf_x[1].ToString()), y_signo, uint.Parse(buf_y[0].ToString()), uint.Parse(buf_y[1].ToString()), z_signo,
uint.Parse(buf_z[0].ToString()), uint.Parse(buf_z[1].ToString()), 0, uint.Parse(grip[0].ToString()),
uint.Parse(grip[1].ToString()), uint.Parse(XYZ[5].ToString()),100, 0, 0, 0, 0);
if (linea.StartsWith("prueba"))
_PicUSBAPI.Envia_Comando(9, 0, x_signo, uint.Parse(buf_x[0].ToString()),
uint.Parse(buf_x[1].ToString()), y_signo, uint.Parse(buf_y[0].ToString()), uint.Parse(buf_y[1].ToString()), z_signo,
uint.Parse(buf_z[0].ToString()), uint.Parse(buf_z[1].ToString()), 0, uint.Parse(grip[0].ToString()),
uint.Parse(grip[1].ToString()), uint.Parse(XYZ[5].ToString()),100, 0, 0, 0, 0);
}
else if (linea.StartsWith("parada"))
else if (linea.StartsWith("paso_ang"))
{
string[] ANG = linea.Split(' ');
byte[] buf_q1 = new Byte[2];

buf_q1 = System.BitConverter.GetBytes(uint.Parse(ANG[1].ToString()));
buf_q7 = System.BitConverter.GetBytes(uint.Parse(ANG[7].ToString())); //delay
_PicUSBAPI.Envia_Comando_Configuracion(8, 2, uint.Parse(buf_q1[0].ToString()),
uint.Parse(buf_q1[1].ToString()), uint.Parse(buf_q2[0].ToString()), uint.Parse(buf_q2[1].ToString()),
uint.Parse(buf_q7[1].ToString()), 0, 0, 0, 0);
}
else
{
MessageBox.Show("Error en el programa", "Mensaje");
return;
}
}
}
private void btn_parada_Click(object sender, EventArgs e)
{
}
}
}
ANEXOS G
Electrnica, Sensores y Estructura del Robot Industrial
Vista Superior de la Tarjeta de Control del Robot Industrial
Vista Inferior de la Tarjeta de Control del Robot Industrial
Detalle de los Sensores del Robot Industrial
Detalle de los Sensores y Codificadores del Robot Industrial en sus

Articulaciones
Robot Industrial Ensamblado

Robotica

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robotica

Uploaded by

Copyright:

Available Formats

126

La robtica es sin duda un rea de inters de las investigaciones de la

matemticas para poder modelar el robot industrial y poder contemplar su

cubicas entre puntos adyacentes para asegurar un movimiento suave sin

Segn la experiencia obtenida con el diseo del control cinemtico y su

Angeles, J., (2007). Fundamentals of Robotic Mechanical Systems, 3ra

Kelly, R., Santibez, V., (2003), Control de Movimiento de Robots

Campa, R., (2005). Control de Robots Manipuladores en Espacio de Tarea,

Investigacin Cientfica y de Educacin Superior de Ensenada, Baja

Retroalimentacin Visual, Tesis de maestra realizada en el Centro de

Panamericano de Ingeniera Naval, Chile.

(2002). Accidentes de la Mano en

Trabajadores de la Costa Oriental del Lago de Maracaibo del Estado

rea de Prensas Troqueladoras de Gurimetal C.A

Prensas Troqueladoras de Gurimetal C.A

Base del Robot Industrial y Electrnica

Detalle del Brazo del Robot Industrial y Pinza

Detalle de la Pinza del Robot Industrial

Vista Superior del Robot Industrial

Vista Lateral del Robot Industrial

Vista Transversal del Robot Industrial

Conector USB tipo A

Disposicin pines conector USB tipo A

//tamano en bytes que envia al programa 2 por cada sensor y 2 para

int16 Ticks4Servo01_medio = 18000;

recibe[0] //definimos nombres para manejar mejor el arreglo de recibo

#define resultado01 envia[0] //definimos nombre para manejar mejor el envio

//lee valor analgico

programa[0][3] = make16(servo06_2,servo06_1); //ngulo, normalmente en 0

angulo_muneca_r = radianes( angulo_muneca_d ); //angulo en radianes para los calculos

//se usa t continuo en 1 para cada paso

aux = q[i_aux][3] * ra;

byte[] buf_q5 = new Byte[2];

Vista Superior de la Tarjeta de Control del Robot Industrial

Vista Inferior de la Tarjeta de Control del Robot Industrial

Detalle de los Sensores del Robot Industrial

Detalle de los Sensores y Codificadores del Robot Industrial en sus

Robot Industrial Ensamblado

You might also like