abierta al universidad autonoma …148.206.53.84/tesiuami/uam4362.pdfobjetewos generales q...
TRANSCRIPT
casa abierta al : t Z m 3 C
UNIVERSIDAD AUTONOMA METROPOLITANA
PLANTEL IZTAPALAPA
DIVISIÓN DE CIENCIAS BÁSICAS E INGENIERIA
DEPARTAMENTO DE COMPUTACIóN
DESARROLLO DEL SOFlWARE CON EMULACIóN CRÁFICA TRIDIMENCIONAL Y CONSTRUCCIóN DE LA INTERFAZ (CIRCUITO LóGICO) PARA CONTROLAR UN BRAZO ROBÓTICO DE MOTORES DE CORRIENTE CONTINUA DE CUATRO GRADOS DE LIBERTAD
TESIS QUE PRESENTA LA ALUMNA
ROSA MARíA CORREDOR CARRILLO MATRICULA: 90221628
PARA OBTENER EL GRADO DE:
LICENCIADA EN COMPUTACI~N
ASESOR: DAVID UZZIEL LÓPEZ ILLESCAS
FEBRERO 2001
DESARROLLO DEL SOFTWARE CON EMULACI~N GRÁFICA TRIDIMENCIONAL Y CONSTRUCCI~N DE LA INTERFAZ (CIRCUITO LÓCICO) PARA CONTROLAR UN BRAZO ROB~TICO DE MOTORES DE CORRIENTE CONTINUA DE CUATRO GRADOS DE LIBERTAD
ALUMNA: CORREDOR CARRILLO ROSA MARIA MATRICULA 90221 628
\
OBJETEWOS GENERALES
Q Desarrollar el software y fabricar la interfaz para controlar un brazo mecánico de cuatro ejes de motores de comente continua.
Emular los movimientos del brazo mecánico por medio de un gráfico en 3D.
OBJETIVOS PARTICULARES - 7 \
5 b n)
? 1. Elegir el tipo de interfaz que puede ser utilizada para controlar t. los cuatro motores de corriente continua del brazo mecánico. i l
2. Desarrollar el software de control para dicha interfaz.
3. Incluir en el software la emulación gráfica en 3D del movimiento del brazo mecánico.
4. Diseñar y fabricar el circuito lógico (interfaz) entre la computadora y el brazo mecánico.
INDICE
Página Introducción 1 Descripción del brazo robótico 2 Selección de la interfaz 4
0 Consideraciones generales 4 0 Descripción del puerto paralelo 5 0 Tabla general del puerto paralelo 6 0 Esquema del puerto paralelo 7 0 Descripción de los componentes 8 0 Funcionamiento 9 0 El puerto paralelo como interfaz de salida para controlar los 10
Desarrollo del software 12 Construcción de la interfaz (circuito lógico) 52
0 Etapa de control manual 52 0 Etapa de control de giro 54 0 Etapa de control de potencia 56 0 Diagrama general 59
Manual de usuario 60 0 Conexión y operación de la interfaz 60 0 Instalación y operación del software 62
Bibliografía 64
motores del brazo
La tecnología informática nos permite contar con herramientas cada vez más poderosas enfocadas al desarrollo industrial. El aplicar estas herramientas y realizar investigación en prototipos que puedan tener aplicación práctica en la industria, o en cualquier sector de la sociedad, es una parte medular para el avance económico de un país.
Los brazos de robot son los que mayor participación tienen en todo tipo de procesos industriales, el desarrollo de la programación y el incremento de la potencia de los microprocesadores impulsan más aplicaciones en el mercado y por tanto, se requiere la preparación de personal calificado que pueda dar mantenimiento, desarrolle programas e interfaces, que respondan a las necesidades actuales.
La investigación tendiente a generar prototipos de brazos de robot con diferentes grados de libertad, interfaces y procesos de programación en todos los niveles es una tarea importante y sobre todo genera las bases de un conocimiento que agilita la búsqueda de un prototipo final rentable.
La presente investigación se dirige al control de un brazo de robot de cuatro grados de libertad con motores de corriente continua. El brazo no cuenta con detectores de proximidad ni sensores de ningún tipo, por tanto, la interfaz y el software de control deben poder sustituir en el mayor grado posible estas carencias, si esto se logra, habremos generado un brazo mecánico de bajo costo para aplicaciones sencillas y un material valioso para nuevos proyectos.
. ..
DESCRIPCIóN DEL BRAZO ROBÓTICO
La estructura mecánica de un brazo robótico está formada por un conjunto de cuerpos rígidos, unidos unos a otros de forma móvil en lo que se conoce con el nombre de articulaciones, el brazo robótico se fija al punto de enclavamiento mediante su primer cuerpo llamado base, la cual puede ser fija o móvil. El resto de los cuerpos que conforman el manipulador no deben considerarse en forma independiente, sino conjuntamente con las articulaciones. Normalmente, en los brazos robóticos, únicamente se utilizan dos tipos de articulaciones: giratorias y prismáticas. Cada una de estas articulaciones sólo precisa de un parámetro (ángulo o desplazamiento) para especificar la posición de los cuerpos que une. Dicho parámetro es llamado “grado de libertad”, cumpliéndose que en un brazo robótico el número de grados de libertad coincide con el numero de articulaciones. La mayoria de las articulaciones de los brazos robóticos modernos son de tipo giratorio, por lo que también reciben el nombre de ejes. Cuando se estudia su disposición, el conjunto de elementos y articulaciones de un manipulador se conoce con el nombre de cadena cinemática. La misión de la estructura mecánica tiene una doble función, esto es, posicionar y orientar el elemento terminal en el espacio y con éI, a la pieza o elemento que transporte.
La estructura mecánica del brazo de robot utilizado en este proyecto tiene las siguientes características:
4 grados de libertad o articulaciones en:
Base (mecanismo giratorio) = ql. Tronco del brazo (mecanismo giratorio) = q2. Elemento terminal muñeca (mecanismo giratorio) = q3. Pinzas (mecanismo giratorio) = q4.
43 Dr
2
En este caso, la estructura mecánica de cada elemento puede moverse respecto a aquél al que va fijado por medio de un motor de comente continua. Por los grados de libertad descritos, este brazo consta de cuatro motores con un sistema reductor por medio de engranes para adaptar la relación velocidad/potencia de las articulaciones.
3
S E L E C C I ~ N DE LA INTERFAZ
Consideraciones generales:
En este caso, no existe información de sensores exteriores que me permita tener un sistema de control de posicionamiento exacto, por otra parte, los motores de corriente continua no realizan ángulos de movimiento de precisión como en el caso de los motores paso a paso, entonces simularé el movimiento del brazo, es decir, que el software permita modificar la velocidad del movimiento de rotación de los ejes del gráfico para emular el movimiento real, por otra parte se requiere diseñar una interfaz que pueda controlar los cuatro motores del brazo en función de la información generada por la computadora en pulsos eléctricos para tener un mejor control del movimiento. Inicialmente, la interfaz seleccionada para esta tarea, consistía en una tarjeta prototipo insertada en un slot tipo ISA de la motherboard dentro de la computadora, la generación del software para controlar la tarjeta como las especificaciones técnicas del slot ISA ya habían sido estudiadas cuidadosamente, las complicaciones surgieron al analizar los costos y la fabricación ya que no existe en el mercado este tipo de tarjetas, y por otra parte, muchos motherboard ya no tienen este tipo de slot. Con estas consideraciones al analizar un slot más actualizado (tipo PCI) resultó que era una tarea más compleja y de mayor costo. Antes de iniciar el desarrollo del software se requiere definir completamente la interfaz que será utilizada. La investigación se modificó hacia la utilización de un puerto serial o paralelo, al estudiar cuidadosamente ambos casos, decidí utilizar el puerto paralelo por las características det problema y las ventajas de operación y programación.
4
Descripción del puerto paralelo:
Inicialmente se colocó al puerto paralelo en la tarjeta del "Adaptador de impresora de IBM", o también con la tarjeta del "monitor monocromático y adaptador de impresora de IBM'.
Con la llegada de clones al mercado, se crea un controlador de múltiples entradas y salidas (Multi I/O) donde se instalan controladores de discos, puertos serie, puerto de juegos y el puerto paralelo.
En la actualidad, el puerto paralelo se incluye comúnmente incluido en la placa madre de la computadora (Motherboard). No obstante, la conexión del puerto con el mundo externo no ha sufrido modificaciones. Este puerto utiliza un conector hembra DB25 en la computadora y un conector especial macho llamado Centronic que tiene 36 pines.
Es posible conectar el DB25 de 25 pines al Centronic de 36 pines ya que cerca de la mitad de los pines del Centronic van a tierra y no se conectan con el DB25.
Descripción del conector DB25 del PC y el conector DB25 del Centronic
Conedor O825 hembra del PC conectar macho del Cwdronic al PC 13 12 1110 9 8 7 6 5 4 3 2 1 1 2 3 4 5 6 7 8 9 1 0 1 1 1 2 1 3
a a a a o a a a o a a a a m a m a a a a a m a a
2524232221 20 1918 17161514 1415 161718 1920 21 22232425
El puerto líneas de
paralelo está formado por 17 líneas de señales y señales están formadas por tres grupos:
o 4 Líneas de control o 5 Líneas de estado o 8 Líneas de datos
8 líneas de tierra. Las
En el diseño original las líneas de control son usadas para la interfaz, control e intercambio de mensajes desde el PC a la impresora.
Las líneas de estado son usadas para intercambio de mensajes, indicadores de estado desde la impresora al PC (falta papel, impresora ocupada, error en la impresora).
Las líneas de datos suministran los datos de impresión del PC hacia la impresora y solamente en esa dirección. Las nuevas implementaciones del puerto permiten una comunicación bidireccional mediante estas líneas.
5
Cada una de estas líneas (control, estado, datos) puede ser referenciada de modo independiente mediante un registro.
Los registros del puerto paralelo
Tabla general del puerto paralelo
6
Esquema del puerto paralelo:
El puerto paralelo esquemáticamente, se describe a continuación. Nótese la conexión al bus ISA en la parte izquierda y los registros en la parte derecha.
bus ISA
a9 a8 a? a6 as a4 a3 a2
a31 830 a29 a28 a27 a26 e2J a24 a23 a22
a l l
b13
b14
b;
nu
gm
a: 1 J Q D Z l b: 4-9,1l-l2JS20 b: 22-290
b 3 . D 1
d o ' dl dz d3 d4
d6 d7
do dl d2 d3 d4 ds d6 d7
mniml lakh
-c3
-171 I I I I I
c=Ontrol L a t h xead-backbaer
bl,blO.b31 I 1
INT ENABLE c4
ata O da 1 ata 2 ata 3 ata 4 & S &a6 ata 7
!IlW
k t 1
pe d 'WY
cb Emto rmt S k t O
>jrds a 1 1
n d
2 3 4 5 6 7 X 9
15 13 12 10 11
1 14 16 17
18 19 20 21 22 23 24
G&+- 25
7
Descripción de los componentes
El puerto paralelo originalmente estaba formado por los siguientes componentes:
O 1 Latch para manejar el registro de datos O 1 Buffer para controlar la retroalimentación del registro de datos o 1 Buffer para manejar el registro de estado O 1 Latch para manejar el registro de control O 1 Buffer para controlar la retroalimentación del registro de control O 1 Multiplexor para direccionar los puertos en el bus ISA O 1 Driver bidireccional para conectar con el bus ISA
Funcionamiento
Explicación del funcionamiento mediante el BIOS y el MS-DOS
IBM especificó direcciones base para el puerto paralelo estándar (dentro del espacio de direccionamiento de Entradalsalida del 80x86). El adaptador de impresora podría usar la dirección base 3BCh, o más tarde 378h o 278h.
El BIOS (Basic Input Output System) de IBM crea en el momento de arranque o POST (Power On Seft Test) una tabla en el espacio de la memoria principal (RAM) para 4 direcciones base de puerto paralelo de impresora, estos se almacenan como 4 bytes empezando con la dirección de memoria 408h. Durante el arranque, el BIOS comprueba si hay puertos paralelos en las direcciones base 3BCh, 378h, y 278h, en ese orden, y almacena la dirección base de cualesquiera que hayan sido encontrados en posiciones consecutivas de la tabla. Las posiciones que no son usadas pueden estar en O, o como algunos BIOS lo hacen, le colocan la dirección del primer puerto encontrado.,
Algunos programas pueden ignorar esta tabla, pero esta es usada por lo menos por el propio BIOS (mediante la INT 17 de E/S de impresora) y por el MS-DOS.
El BIOS detecta estos puertos escribiendo AAh al registro de datos (en la dirección de E/S Base + O), y luego si en el registro de datos se lee AAh. Significa que hay un puerto.
8
Normalmente la asignación de direcciones es como sigue:
." .
Dirección Nombre Ubicacibn . ..
3BCh LPTI Adaptador de impresión primario
.. . . . .
378h LPT2 Adaptador de impresión secundario
Las referencias a cada registro del puerto se realizan de la siguiente forma:
O Base (datos)=base+O c Estado=base+l O Control=base+2
Por ejemplo, si encontramos que la dirección base es 378h, entonces las direcciones del registro de datos, estado y control serán:
O Base (datos)=378h O Estado=379h O Control=37Ah
Cada una de ellas permite acceder a los siguientes bits (descritos en la tabla general):
0 Base (datos)=DO, D l , D2, 03, D4, D5, D6, D7 O Estado=S3, S4, S5, S6, S7 O Control=CO, C1, C2, C3
9
El Puerto Paralelo como lnterfaz de salida para controlar los motores del brazo
Con los ocho bits de salida de datos podemos controlar los cuatro motores del brazo y el giro de cada uno independientemente.
BUSY OUT OF PAPER
SELECT
AUTO FEED ERROR
INITIALIZE
SELECT
* 13 12 11 10
GROUND x
Como ya se indico se puede, por medio del primer 110 port, enviar señales al dispositivo conectado en el puerto de salida. Para ello debemos entender que cada uno de los ocho bits de este puerto, se corresponde con un pin de los destinados a datos, que son aquellos cuya numeración va de la 2 a la 9. De este modo podemos establecer la relación de la siguiente manera:
En términos generales, para mandar una señal por un pin, lo que tenemos que hacer es activar su bit correspondiente (o lo que es lo mismo, poner dicho bit a uno). Con esto se consigue una señal de 5 voltios por el pin deseado, la computadora seguirá enviando esa señal hasta que pongamos ese bit a O.
10
Hasta este punto, el puerto paralelo nos ofrece la mejor opción de control, el siguiente paso es el generar esta información por medio de un lenguaje de programación. En este caso, lenguaje C permite una programación adecuada del puerto y construcción del gráfico. La idea inicial es generar un control por medio de las teclas de funciones de la computadora que correspondan al movimiento de cada motor del brazo y generar el gráfico, así mismo, el programa debe poderse modificar para cambiar la velocidad del gráfico en la pantalla y emular lo mejor posible al modelo real.
11
PROGRAMA DE BRAZO MECÁNICO LENGUAJE C++
12
/* Programa de Brazo menanico */
#include <graphics. h> #include <math.h> #include <alloc.h> #include <conio.h> #include <stdlib.h> #include <stdio.h> #indude <dos.h>
int TARJE = 0x0378;
float PI = 3.14159; . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
‘ ‘cons *I . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I unsigned int fig[8][16] = {
P gira izq *I
(Ox0020 , 0x0040 ,Ox0080 ,oxo100 ,ox0201 ,Ox0402 ,OxOCO4 ,Ox1208 ,Ox2510 ,Ox4AAO ,ox54co ,Ox5900 ,ox2200 ,ox1 coo ,ox0000 ,0xo000}
P gira der * I
,{Ox0800 ,Ox0400 ,oxo200 ,ox0100 ,Ox0080 ,Ox8040 ,Ox4040 ,Ox2060 ,ox1 O90
13
, 0x0948 ,Ox06A4 ,Ox0254 , Ox0 1 34 ,Ox0088 ,Ox0070 ,0xo000>
I Baja brazo */
,{Ox0000 ,ox0000 ,ox0000 ,ox0000 ,oxoooo ,ox0000 ,oxo000 ,Ox3COO ,Ox43FF ,Ox5AOI ,Ox5A01 ,Ox43FF ,Ox4200 ,Ox8100 ,OxFFOO ,Ox0000}
r* Sube brazo * I
,{Ox0008 ,Ox0014 ,ox0022 ,ox0042 ,Ox0084 ,Ox0108 ,ox02 1 o ,Ox3C20 ,Ox4240 ,Ox5A80 ,Ox5B00 ,Ox4200 ,Ox4200 ,Ox8100 ,OxFFOO ,0x0000}
14
P' sube mano *I
,{Ox0000 ,ox001 c , 0x0024 ,Ox0044 ,Ox0088 ,oxo1 10 ,OxFFEO ,Ox002Q ,ox0020 ,OxFFCQ ,ox0000 ,ox0000 ,ox0000 ,ox0000 , oxoooo ,0x0000}
P' baja mano * I
,{Ox0000 ,ox0000 ,oxo000 ,ox0000 ,oxo000 ,oxo000 ,OxFFCO ,ox0020 ,ox0020 ,OxFFCO ,Ox0880 ,ox1 1 O0 ,ox2200 ,Ox2400 ,Ox3800 ,Ox0000}
P' Cierra pinza *I
,(Ox1 o1 o ,ox1010 ,ox1010 ,ox1 o1 o ,ox1 o1 o ,ox1 o1 o ,ox1010 ,Ox1 FFO ,Ox1 290 ,Ox1 290 ,ox1 290
15
,Ox1390 ,Ox0920 ,oxo540 ,Ox0380 ,0x0000}
P' Abre pinza */
,(Ox1 o1 o ,ox1010 ,Ox? o1 o ,ox1 o1 o ,ox1010 ,ox1 o1 o ,ox1010 ,Ox1 FFO ,Ox1 290 ,Ox2448 ,Ox4824 ,ox9012 ,Ox50 14 ,Ox3018 ,oxo000 ,OxoOOO) 1;
............................................................................................. I
.............................................................................. I P' Variables del experimento */
P' para la pantallita *I
I* para los botones de control */
int bx[8]; int by[8];
unsigned char movim; unsigned char ctl-movim; unsigned char ctl-boton;
struct time reg-h[4]; struct time pos-h[4]; struct time max-h[4]; struct time stoph[4];
struct time mov-h[4];
16
P' representacion del grafico - coordenadas cartecianas y polares *I
struct VECTOR-XYZ {
double x,y,z;
struct VECTOR-POL {
double r,t,f; 1;
double a,b,c,d,dval; struct VECTOR-XYZ from, at, up; double angle; struct VECTOR-XYZ a l , a2, a3; struct VECTOR-XYZ g; int connect[lOO]; struct VECTOR-XYZ pointarray[l50]; int length; int vertices; double obi_x-min,obix-max; double obj_y_min,objv-max; double obj_z_min,obj_z_max; double t-from; double offs-x, o f f s ~ , offs-z; stnrct VECTOR-XYZ dist; int max-x, max_y; int pcb,pct,pcl,pcr;
struct VECTOR-POL vbase,vbraz,vmano,vpinza; struct VECTOR-POL v;
OPERACIONES CON VECTORES
.................................................................... I
double mag(struct VECTOR-XYZ 'v) {
17
I
void substract(stntct VECTOR-XYZ *vl ,struct VECTOR-XYZ *v;l,struct VECTOR-XYZ * a {
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I void cross(struct VECTOR-XYZ *vl ,struct VECTOR-XYZ *v;l,stmct VECTOR-XYZ *c) {
r*C*H**H******C**.*****m********C****** I void divide(struct VECTOR-XYZ *v, double num,struct VECTOR-XYZ *d) {
if (num == O) {
printf("Error: divicion por cero en matriz"); exit(1);
} else {
d->x = v->x / num; d->y = v->y / num; d->z = v->z / num;
F(TNC1c)NF.S VARTADAS /******n**m********mCH*HH**H********** I
r' maximo de una variable *I
double maxof(double vall ,double va12) { double aux;
if (vall > va12) { aux = vall;
18
} else {
1 aux = va12;
return (aux); 1 P*********C************************* I
P conversion a radianes *I
double torad(double grados) { double aux;
aux = grados * 0.017453293;
return (aux); 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I
P correspondencia del mundo real a la PC *I
void WORLMoPC(doub1e x w , double y w , int *xpc, int 'ypc) {
*xpc = (int)(a xw + b);
*ypc = (int)(c yw + d);
}
r******wc*m***c**n******c lN*r r r rwt t****c I r* minimo y maximo de un numero *I
void rninrnaxo { int i;
obix-min = 32000; obu-min = 32000; obkz-min = 32000; obix-max =-32000; obo-rnax =-32000; obiz-max =-32000;
for (i=O;i<vertices; i++) {
if (pointarray[i].x > obj_x_rnax ) obj-x-max = pointarray[i].x; if (pointarray[i].x e obix-rnin ) obj-x-min = pointarray[i].x;
if (pointarray[i].y > obu-rnax ) obu-rnax = pointarray[i].y; if (pointarray[i].y e obo-rnin ) obo-rnin = pointarray[i).y;
if (pointarray[i].z > obLz-rnax ) obj-z-max = pointarray[i].Z; if (pointarray[i].z e obiz-min ) obj-z-rnin = pointarray[i].z;
19
," ~ .~ ~ .. ~ .. ~ ~. "" "" ~ . " ~
r' estado del grafo en la computadora *I
void setat() {
minmax();
at.x = (obLx-min + obix-max) / 2; at.y = (obu-min + obu-max) / 2; at.z = (obj-z-min + obj-z-max) / 2;
r***H********H*************************~**HHHH-****~**~**~~~*****H*~**H~****~ I /* posici6n en el mundo real *I
const double WIDTH = 1.8;
void setfrom0 {
fr0m.x = at.x + (obj-x-max - obj-x-min) / 2.0 + WIDTH maxof ((obj-z-max - obj-z-min) / 2.0,
(obo-max - obo-min) / 2.0); from.y = at.y; from.z = at.2;
............................................................................... I P referencia de vision - perspectiva de vision */
void seteye() {
double amarkmag,tempmag; struct VECTOR-XYZ temp;
dval = cos(angleR.0) / sin(angleR.0); substract(&at , &from, &dist);
amarkmag = mag(&dist);
divide(&dist , amarkmag, &a3); cross(&dist , &up, &temp); tempmag = mag(&temp);
divide(&temp , tempmag, &al);
20
cross(&al , &a3, &temp); tempmag = mag(&temp);
divide(&temp , tempmag, &a2);
offs-x = -al.x fr0m.x - a1.y * fr0m.y - a l .z * fr0m.z; offs-y = -a2.x from.x - a2.y * fr0m.y - a2.z * fr0m.z; offs-z = -a3.x * fr0m.x - a3.y fr0m.y - a3.z * fr0m.z;
1
const int NOEDGE = 0x00; const int LEFTEDGE = 0x01; const int RIGHTEDGE = 0x02; const int BOTTOMEDGE = 0x04; const int TOPEDGE = 0x08;
int code (double x, double y, double z) { int c;
c = NOEDGE;
if (X < -2) c I= LEFTEDGE; if (x > z) c I= RIGHTEDGE;
if (y > z) c I= TOPEDGE; if (y < -2) c I= BOTTOMEDGE;
retum(c); 1 2 2 5 8 9 2
I
I* traza el vector en 3D *!
void dip3d(double XI , double y1 , double z l
int c,cl ,c2,xpcl ,ypcl ,xpc2,ypc2; double x,y,z,t;
,double x 2 , double y2, double 22) {
c l = code(x1 ,y1 ~ 1 ) ; c2 = code(xZ,y2,22);
while (c l != NOEDGE 11 c2 != NOEDGE) {
if ((cl & c2) != NOEDGE) return;
c = c l ;
if (c == NOEDGE) c = c 2 ;
21
P cruza orilla izq */ if ((c & LEFTEDGE) == LEFTEDGE) {
t = (21 + x?) /((XI - x2) - (22 - 21)); z = t * ( z 2 - z l ) + z l ;
y = t * ( y 2 - y l ) + y l ; x = -2;
} else
P cruza orilla der */ if ((c & RIGHTEDGE) == RIGHTEDGE) {
t = (z l - X I ) / ((x2 - x l ) - (2.2 - zl)); z = t * ( z 2 - z l ) + z l ;
y = t * (y2 - y l ) + y l ; x = 2;
} else
P cruza orilla abajo */ if ((c & BOTTOMEDGE) == BOTTOMEDGE) {
t = (z l + y l ) / ((y1 - y2) - (22 - 21)); z = t * ( z 2 - z l ) + z l ; x = t * ( x 2 - x l ) + x l ; y = -z;
} else
P cruza orilla arriba */ if ((c & TOPEDGE) == TOPEDGE) {
t = (21 - y l ) / ((y2 - y l ) - (2.2 - zl)); z = t * ( z 2 - z l ) + z l ; x = t * ( x 2 - x l ) + x l ; y = z;
if (c == c l ) {
x1 =x; Y 1 =y; z l = 2;
c l = code(x,y,z);
) else {
x2 = x; Y2 = y;
22
22 = z; c2 = code(x,y,z);
if (z l != O) { WORLDtoPC(xl/zl ,yl/zl,8apcl ,&ypCl); WORLDtoPC(x2/z2,y2/z2,&xpc;!,&ypc2);
WORLDtoPC(x1 ,y1 ,&xpcl ,&ypCl); WORLDtoPC(x2 ,y2 ,&xpc2,&ypc2);
} else {
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I P transforma los puntos a segmentos - vectores completos *I
void transformseg(struct VECTOR-XYZ *VI, strut3 VECTOR-XYZ *v2) {
double x1 ,y1 ,z l ,x2,y2,z2;
x1 = (vl->x * a l .x + a l .y vl->y + a1.z vl->z + offs-x) dval; y1 = (VI->x a2.x + a2.y vl->y + a2.z * vl->z + offsqr) * dval; z l = (vl->x a3.x + a3.y * vl->y + a3.z * vl->z + offs-z);
x2 = (v2->x a l .x + a l .y v2->y + a l .z v2->z + offs-x) dval; y2 = (v2->x a2.x + a2.y v2->y + a2.z * v2->z + o f f s ~ ) * dval; z2 = (v2->x a3.x + a3.y * v2->y + a3.z v2->z + offs-z);
clip3d(xl ,y1 ,z l ,x2,y2,z2);
1 ........................................................................ I
P borra pantalla grafica *I
void clear-viewport() {
setviewport(pcl,pct,pcr,pcb,l); clearviewport(); setwewport(O,O,getmaxx(),getmaxy(),l);
23
p\H*tr*~m*i******.*H****t******* I r" transforma los segmentos hacia diferentes posiciones *I
void setview() { int i;
i = 1; while (I vertices) {
transformseg(&pointarray[i-l],&pointarray[i]); I++;
1;
I
r' dibuja los vectoms */ I
void linexyz(struct VECTOR-XYZ *pl, struct VECTOR-XYZ *p2) { int xpcl ,ypcl ; int xpc2,ypc2;
/* xpcl=( (gQ+gxl) / 2) - pl->x; ypcl= gy2 - (pl->z / 2) + (pl->y /2);
xpc2=( (gx2+gxl) / 2) - p2->x; ypc2= gy2 - (p2->z / 2) + (p2->y /2); */
xpcl=( (gx2+gxl) / 2) - pl->x; ypCl= gy2 - (pl->Z ) -50 ;
xpc2=( (gX2+gxl) / 2) - pZ->x; ypc2= gy2 - (P~->z ) -50 ;
I
I* funcion suma de vectores */
void sumavector(struct VECTOR-XYZ *a, struct VECTOR-POL *w, struct VECTOR-POL 'V
,struct VECTOR-XYZ *d, struct VECTOR-POL *9 {
f->r = O; f->t = w->t + v->t; f->f = w->f + v->f;
24
d->x = a->x + ( v->r ( sin(f->f) cos(f->t) ) ); day = a->y + ( v->r * ( Sin(f->f) Sin(f->t) ) ); d->z = a->z + ( v->r ( cos(f->f) ) );
FASE 1: I DIBUJO Y GIRO DE LA BASE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I
r" dibuja el tronco *I
void draw-fase1 (int opt) { struct VECTOR-POL k; struct VECTOR-XYZ pfin; struct VECTOR-POL vfin; float max-t; float paux; float maux;
struct VECTOR-XYZ taux; struct VECTOR-POL auxl ;
P*** limites del brazo
min max
t o pi f - -
r" inicializa la pocision de soporte *I
g.x = o; g.y = o; g.z = o;
v.r = O; v.t = o; v.f = o;
max-t = PI;
/* calcula posicion del vector en relacion al tiempo "I
paux = (mov-h[O].ti-sec 100) + mov-h[O].tjhund; maux = (max-h[O].ti-sec 100) + max-h[O].ti-hund;
k.r = 30; k.t = max-t * (paux) I maux; k.f = O;
25
setcolor( 14);
switch (opt) {
case O: auxl .r = 3; auxl .t = (max-t (paux) / maux)-(P1/2); auxl .f = (PIE); sumavector(&g ,&v,&auxl ,&taux,&vfin); linexyz(&g ,&taux);
sumavector(&taux,&v,&k ,&pfin,&vfin); linexyz(&taux,&pfin);
aux1.r = 3; aux1.t = (max-t (paux) / maux)+(P1/2); aux1.f = (P1/2); sumavector(&g ,&v,&auxl ,&taux,&vfin); linexyz(&g ,&taux);
sumavector(&taux,&v,&k ,&pfin,&vfin); linexyz(&taux,&pfin);
sumavector(&g,&v,&k,&pfin,&vfin); linexyz(&g,&pfin); break;
case 1 : sumavector(&g,&v,&k,&pfin,&vfin); linexyz(&g,&pfin); break;
v = vfin; r" ultima direccion *I
g = pfin; r* posicion donde se quedo el punto de soporte *I
FASE 2: DIBUJO Y ELEVACION DEL BRAZO
.............................................................................................. I
r" dibuja el brazo *I
void draw_fase2(int opt) { struct VECTOR-POL k; struct VECTOR-XYZ pfin;
26
struct VECTOR-POL vfin; float ma-f; float paux; float rnaux;
struct VECTOR-XYZ taux; struct VECTOR-POL auxl;
max-f = PI /2;
P' calcula posicion del vector en relacion al tiempo */
paux = (mov-h[l].ti-sec 100) + rnov-h[l].ti-hund; maux = (max-h[l].ti-sec * 100) + max-h[l].tjhund;
k.r = 50; k.t = O; k.f = max-f (rnaux - paux) / maux;
setcolor( 13); switch (opt) {
case O: aux1.r = 3; auxl .t =-(P1/2); auxl .f = (PIE); sumavector(&g ,&v,&auxl ,&taux,&vfin); linexyz(&g ,&taux);
sumavector(&taux,&v,&k ,&pfin,&vfin); linexyz(&taux,&pfin);
auxl .r = 3; auxl .t = (PIE); aux1.f = (PIE); sumavector(&g ,&v,&auxl ,&taux,&vfin); linexyz(&g ,&taux);
sumavector(&taux,&v,&k ,&pfin,&vfin); linexyz(&taux,&pfin);
sumavector(&g,&v,&k,&pfin,&vfin); linexyz(&g,&pfin); break;
case 1: sumavector(&g,&v,&k,&pfin,&vfin); linexyz(&g,&pfin); break;
27
1;
v = vfin; P' ultima direccion * I
g = pfin; P' posicion donde se quedo el punto de soporte * I
1
P' dibuja la muñeca *I
void draw_fase3(int opt) { struct VECTOR-POL k; stnrct VECTOR-XYZ pfin; struct VECTOR-POL vfin; float max-f; float paux; float maux; float off set;
struct VECTOR-XYZ taux; struct VECTOR-POL auxl ;
P*** limites del brazo;
min max
offset = P1/4; max-f = PI;
P' calcula posicion del vector en relacion al tiempo */
paux = (mov_h[2].ti_sec * 100) + mov_h[2].ti_hund; maux = (rnax-h[2].ti_sec 100) + rnax-h[2].ti_hund;
k.r = 8; k.t = O;
k.f = max-f (paux) / maux;
if (k.f offset) { k.f = 2 * PI - (offset - k.9; else { k.f = k.f - offset;
28
1 setcolor( 1 O);
switch (opt) {
case O: aux1.r = 5; auxl .t =-(PIE); aux1.f = (P1/2); sumavector(&g ,&v,&auxl ,&taux,&vfin); linexyz(&g ,&taux);
sumavector(&taux,&v,&k ,&pfin,&vfin); linexyz(&taux,&pfin);
aux1.r = 5; auxl .t = (PV2); aux1.f = (PV2); sumavector(&g ,&v,&auxl ,&taux,&vfin); linexyz(&g ,&taux);
sumavector(&taux,&v,&k ,&pfin,&vfin); linexyz(&taux,&pfin);
sumavector(&g,&v,&k,&pfin,&vfin); linexyz(&g,&pfin); break;
case 1: sumavector(&g,&v,&k,&pfin,&vfin); linexyz(&g,&pfin); break;
v = vfin; /" ultima direccion * I
g = pfin; r* posicion donde se quedo el punto de soporte */
I ." ..
r" dibuja pinza izquierda *I
void draw_fase4(int opt) { struct VECTOR-POL k; struct VECTOR-XYZ pfin; struct VECTOR-POL din;
29
float max-f I float paux; float maux:
struct VECTOR-XYZ taux; struct VECTOR-POL auxl ;
P*** limites del brazo;
min max
t -PI12 -PI12 f O P1/2 */
max-f = PIG':
r* calcula posicion del vector en relacion al tiempo */
paux = (mov_h[3].ti_sec * 100) + mov-h[3].t¡-hund; maux = (max-h[3].ti_sec * 100) + max-h[3].ti_hund;
k.r = 15; k.t =- max-f (paux) / maux;
k.f = O;
setcolor( 1 1); switch (opt) {
case O:
sumavector(&g,&v,&k,&pfin,&vfin); linexyz(&g,&pfin);
auxl .r = 3; auxl .t =-(P1/2); auxl .f = (PV2);
sumavector(&g ,&v,&auxl ,&taux,&vfin); linexyz(&taux,&pfin); aux1.r = 1; auxl .t =-(P1/2); auxl .f = (P112); sumavector(&g ,&v,&auxl ,&taux,&vfin); linexyz(&taux,&pfin);
break;
case 1: sumavector(&g,&v,&k,&pfin,&vfin); linexyz(&g,&pfin); break;
FASE 5: DIBUJO Y ELEVACI6N DE PINZA DERECHA
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I r‘ dibuja pinza derecha *I
void draw_fase5(int opt) { struct VECTOR-POL k; struct VECTOR-XYZ pfin; struct VECTOR-POL vfin; float max-f; float paux; float maux;
2 2 5 8 9 2
struct VECTOR-XYZ taux; struct VECTOR-POL auxl ;
Y** limites del brazo ******
min max
t -PI12 -PI12 f O PI12
ma-f = PV2; “****lr************ I
/* calcula posicion del vector en relacion al tiempo *I
paux = (rnov-h[3].ti-sec 100) + mov_h[3].ti_hund; maux = (rnax-h[3].tjsec 100) + rnax-h[3].tjhund;
k.r = 15; k.t = max-f (paux) / maux;
k.f = O;
setcolor( 12);
switch (opt) {
case O:
sumavector(&g,&v,&k,&pfin,&vfín);
linexyz(&g,&pfin);
aux1.r = 3; auxl .t = (P1/2); aux1.f = (PIE); sumavector(&g ,&v,&auxl ,&taux,&vfin); iinexyz(&taux,&pfin); aux1.r = 1; aux1.t = (PIE); aux1.f = (P1/2); sumavector(&g ,&v,&auxl ,&taux,&vfin); linexyz(&taux,&pfin);
break;
case 1 : sumavector(&g,&v,&k,&pfin,&vfin); linexyz(&g,&pfin); break;
DIBUJO COMPLETO TODAS LAS FASES
r********”**m**mrm****cmH****lr I P CONTROL COMPLETO DEL DIBUJO */
void draw-brazo(int opt) {
draw-fase1 (opt); draw_fase2(opt); draw_fase3(opt); draw-fas&(opt); draw_fase5(opt);
void ctl-sim() {
int i; if ( (movim) 11 (ctl-movim) ) {
if (movim) { draw-brazo(1); P borra fig *I
32
unsigned int size;
size=imagesize(xl ,y1 ,x2,y2); if ( (ppp = malloc(size))!=NULL ) {
getimage(x1 ,y1 JQ,Y~,PPP); putimage(x1 ,y1 ,ppp,NOT-PUT); free(PPP);
I I . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I
P' inicializa pantalla grafica */
void inigraficos() { int gd,gm; gd=VGA; gm=VGAHI;
initgraph(&gd,&gm,""); if ( graphresult() != grOk ) { I* inicializa ambiente grafico */
clrscr(); printf("ERR0R: No es posible instalar el ambiente graficos h"); sieep(1);
printff' presione cualquier tecla.. ."); getch0; printf('7nh"); exit( 1);
1 I r*H***H********H*******HHH**************************~**************HH**********H***********~ I
/" dibujo de icons (botones) *I
void paint-icn(int nfigjnt x, int y) { int i,j; unsigned int bit;
for (i=O;i<l6;i++) {
bit = 1:
for (j=O;j<16;j++) {
if (bit & fig[nfig][i]) {
putpixel(x+l5-j,y+i, 1);
34
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I
r* DIBUJO DE PANTALLAS Y BOTONES *I
r * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ~ * * * * * * * * ~ * * * * ~ ~ ~ ~ * ~ ~ * * * ~ I
void draw-pantallita0 {
setcolor( 15); rectangle(gxl-l3,gyl-13,gx2+13,~y2+13);
line (380,340,600,120);
setfillstyle (SOLID-FILL,O); floodfill(381,121,15); setfillstyle (SOLID_FILL,8); floodfi11(593,332,15);
setcolor(0); rectangle(388,128,592,332); setcolor(7); line (380,340,600,120);
line (380,120,600,340);
setcolor(YELL0W); rectangle(gx1-1 ,gyl-1 ,gx2+1 ,gy2+1);
clear-viewport();
.................................................................................................. I
void draw-botones() { int i;
for (i=O;i<8;i++) { bottom(b~i]-20,by[i]-20,bx[i]+20,by[i]+20); paint_icn(i,bx[i]-7,by[i]-l7);
35
setcolor( O); settextstyle(SMALL-FONT,0,4); settextjustify(CENTER-TEXT,CENTER-TEXT);
void pantalla() { int i ;
setcolor(0xF); setfillstyle (SOLID_FILL,7); baed( 30,110,610,350,0,1); setcolor(0); rectangle( 31 ,I 11,609,349);
7" Pantallita "**"*-** I
draw-pantallita();
draw-botones();
....................................... I void togle-boton(int bx, int by, unsigned char valor) { unsigned char mask; unsigned char tempo; int i;
tempo=l ; mask =O; for (i=O;i<8;i++) {
if (!(tempo & valor)) {
mask = mask I tempo;
36
1 tempo *= 2;
1
if (ctl-boton & valor) {
ctl-boton = ctl-boton & mask;
} else {
ctl-boton = ctl-boton I valor;
3
1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I
r" CONTROL DEL MOVIMIENTO DEL MOTOR * I
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I
void togle-movim(unsigned char valor) { unsigned char mask; unsigned char tempo; int i;
tempo= I ; mask =O; for (i=l;i<=8;i++) {
if (!(tempo & valor)) {
mask = mask I tempo;
I tempo *= 2;
if (ctl-movim & valor) {
ctl-movim = ctl-movim & mask;
} else {
ctl-movim = ctl-movim I valor;
1
CON RESPECTO A LA VARlABLE "VALOR PARA DETENERLO O ARRANCARLO AL PRESIONAR EL BOTON
I I
37
tempo=l ; mask =O; for (i=l;ic=8;i++) {
if (!(tempo & ctl-movim)) {
mask = mask I tempo;
1 tempo *= 2;
1 outport(TARJE,mask);
AL PUERTO (SBITS) - TARJE ES LA DIRECCION DEL PUERTO.
I I
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I
void control-botones(int b) { unsigned char princ; unsigned char depen;
switch (b) { case O: princ=Ox01; case 1: princ=Ox02; case 2: princ=Ox04; case 3: princ=Ox08; case 4: princ=OxlO; case 5: princ=Ox20; case 6: princ=Ox40; case 7: princ=Ox80;
1
depen=Ox02; depen=Ox01; depen=Ox08; depen=Ox04; depen=Ox20; depen=Oxl O; depen=Ox80; depen=Ox40;
break; break; break; 8 BOTONES PARA CONTROLAR LA break; ROTACION IZQUIERDA Y DERECHA
break; QUE SE PRESIONA- break; DE LOS MOTORES SEGUN EL BOTON
break; break;
if (!(ctl-boton 8, depen)) {
togle-boton(bx[b],by[b],princ);
1
1; r*c********m*******H**HH***WH********* I
P' funciones para el control del tiempo *I P' aux = b - a */
void resta-time(struct time b,struct time a,struct time *aux) {
if (b.ti-hund >= a.ti-hund) {
38
(‘aux).ti-hund=b.ti-hund - a.ti-hund;
} else {
(*aux).tjhund=(b.tjhund) + (100 - a.tjhund);
a.ti-sec ++;
1 if (b.ti-sec >= a.ti-sec) {
(*aux).ti-sec =b.ti-sec - a.ti-sec;
} else {
(*aux).ti-sec=(b.ti-sec) + (60 - a.ti-sec);
a.ti-min ++;
}
if (b.tjmin >= a.ti-min) {
(*aux).ti-min=b.ti-min - a.ti-min;
} else {
(*aux).ti-min=(b.ti-min) + (60 - a.ti-min);
a.ti-hour++;
1 if (b.ti-hour >= a.ti-hour) {
(‘aux).tjhour=b.tjhour - a.tjhour;
} else {
(*aux).ti-hour=(b.ti-hour) + (24 - a.ti-hour);
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I /* aux = b + a *I
void suma-time(struct time b,struct time a,struct time *aux) {
39
(*aux).tjhund=O; (‘aux).ti-sec =O; (‘aux).ti-min =O; (*aux).ti-hour=O;
(*aux).ti-hund = (‘aux).ti-hund + b.ti-hund + a.ti-hund;
if ((‘aux).ti_hund>=lOO) { (‘aux).ti-hund = (‘aux).ti-hund - 100; (*aux).ti-sec = (*aux).ti-sec + 1;
1
(*aux).ti-sec = (*aux).ti-sec + b.tjsec + a.ti-sec ;
if ((*aux).t¡-sec>=60) { (‘aux).ti-sec = (‘aux).tjsec - 60; (‘aux).ti-min = (‘aux).ti-min + 1;
1
(‘aux).ti-min = (‘aux).ti-min + b.ti-min + a.ti-min ;
if ((‘aux).t¡-min>=60) { (‘aux).ti-min = (‘aux).ti-min - 60; (‘aux).ti-hour = (‘aux).ti-hour + 1;
1
(‘aux). t jhour = (*aux). ti-hour + b. t jhour + a. tjhour;
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I r“ compara tiempos de movimiento - limites *I
int comp-time(struct time a,unsigned char simbo1,struct time b) {
int aux=l; int tmp=l;
switch (simbol) { case ‘<’: if (tmp == 1) {
trnp = O; if (a.ti-hour b.ti-hour) { aux=l; } if (a.ti-hour == b.tjhour) { trnp=l; } if (a.tjhour > b.ti-hour) { aux=O; }
1
if (trnp == 1) { tmp = O; if (a.tjmin b.ti-min ) { au-1; }
40
if (a.ti-min == b.ti-min ) { tmp=l; } if (a.ti-min > b.ti-min ) { aux=O; }
1 if (tmp == 1) {
trnp = O; if (a.ti-sec .= b.ti-sec ) { au-1; } if (a t jsec == b.ti-sec ) { tmp=l ; } if (a.ti-sec > b.ti-sec ) { aux=O; }
1 if (tmp == 1) {
tmp = O; if (at¡-hund < b.ti-hund) { a w l ; } if (a.ti-hund == b.ti-hund) { aux=O; } if (a.ti-hund > b.ti-hund) { au-0; }
1
break;
case ' = I : aux=l; if (a.tjhund != b.tjhund) { aux=O; } if (a.ti-sec != b.ti-sec ) { aux=O; } if (a.ti-min != b.tjmin ) { aux=O; } if (a.ti-hour != b.ti-hour) { aux=O; }
break;
case '>I: if (tmp == 1) { tmp = O; if (a.ti-hour .= b.ti-hour) { aux=O; } if (a.ti-hour == b.ti-hour) { tmp=l; } if (a.ti-hour > b.tjhour) { aux=l; }
1 if (tmp == 1) {
tmp = O; if (a.ti-min < b.ti-min ) { aux=O; } if (a.tjmin == b.ti-min ) { tmp=l ; } if (a.ti-rnin > b.ti-min ) { aux=l; }
1 if (trnp == 1) {
tmp = O; if (a.ti-sec < b.tjsec ) { aux=O; } if (a.ti-sec == b.ti-sec ) { tmp=l; } if (a.ti-sec > b.ti-sec ) { aux=l; }
1
if (trnp == 1) {
41
tmp = O; if (a.ti-hund b.tjhund) { aux=O; } if (a.ti-hund == b.ti-hund) { aux=O; } if (a.ti-hund > b.tjhund) { aux=l; }
1
break; 1 retum(aux);
}
void reg-motor-time(int ijnt j) {
int imotor; struct time auxd; struct time auxf; struct time timenow;
imotor = (int) (¡a);
gettime(&timenow);
i f t i== 1){
suma~time(pos~h[imotor],auxd,&auxf); if (comp-time( pos-h[imotor] , ' > I , max-h[imotor] )) {
pos-h[imotor] = max-h[imotor];
} else {
pos-h[imotor] = auxf;
} else {
if ( comp~time(pos~h[imotor],'~',auxd)) {
resta~time(pos~h[imotor],auxd,&auxf); pos-h[imotor] = auxf;
} else {
42
pos-h[imotor].ti-hund=O; pos-h[imotor].ti-sec =O; pos-h[imotor].ti-min =O; pos~h[imotor].tjhour-O;
reg-h[imotor]=timenow;
P gotoxy(lO,imotof4+1); printf("pos-h : %2d:%2d:%2d.%2d",pos-h[imotor].ti-hour,
pos-h[imotor].ti-min, pos~h[imotor].ti~sec, pos-h[imotor].ti-hund);
gotoxy(1 O,imotor*4+2);
printf("reg-h : %2d:%2d:%2d.%2d",reg-h[imotor].ti-hour, reg-h[imotor].tjmin, reg-h[imotor].tjsec, reg-h[imotor].ti-hund);
gotoxy(lO,imotof4+3); printf("max-h : %2d:%2d:%2d.%2d",max-h[imotor].ti-hour,
max-h[imotor].ti-min, max-h[imotor].ti-sec, max-h[imotor].ti-hund);
*I
pI*m***L*******H***c*****tm**w*n**x******~ I I" arranca el motor en un sentido u otro *I
void start-motor(int i,int j) { int imotor; strut3 time aux;
imotor = i M ; gettime(®-h[imotor]);
if (j == O) {
} else {
43
P***********************************~**********************************************************~ I I* pregunta el limite para todos *I
int llego-limite(int i,int j) { struct time aux; int imotor; int tmp;
imotor = i/2;
i f ( j = = O ) {
aux.ti-hour = O; aux.tjmin = O; aux.ti-sec = O; aux.ti-hund = O;
tmp = (comp-time( pos-h[imotor] ,'=I, aux ));
1 else {
tmp = (! comp-time( pos-h[imotor] ,'e', ma-h[imotor] )); } return (tmp);
1
pmt***LL***Hlr******H**H**************** I P - ctl-movim P I - ctl-boton r* I I - ctl-movim
r* P O 1 O no hay cambio por llegar al limite r* O 1 1 arranca el motor solicitado r* 1 1 O detiene monitor por llegar al limite r" 1 1 1 mantiene el motor en movimiento P r* 1 O O detiene motor r* O 0 0 no hay cambio *I r********************************************************* I void ctl-mov() { int i,j; unsigned char bit;
r* I l l
bit = 1;
44
for (i=O;i<8;i=i+Z) { for (j=O;j<Z;j++) {
if (bit & ctl-movim) {
reg-motor-time(i,j);
if (bit & ctl-boton) {
if (llego-limite(i,j)) {
togle-movim(bit);
control-botones(i+j);
} else {
P' registra tiempo de motor * I
r llego al limite */
P' para motor * I
P' desactiva boton*l
r* no se realiza nada */
} else {
r' para motor *I togle-movim(bit);
1
} else {
if (bit & ctl-boton) {
if (llego-limite(i,j)) {
control-botones(i+j);
} else {
P' llego al limite *I
P desactiva botan*/
P' calcula la hora de paro */
P arranca motor *I start-motor(i,j); togle-movim(bit);
1 } else {
1 P' no se realiza nada *I
45
bit = bit 2;
} r* for *I } r for */
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . I r* teclas a presionar *I
int ctl-bot() { unsigned char tecla; int fin = O:
if (kbhit()) { tecla = getch();
switch (tecla) {
case 27: fin = 1; break;
case O: tecla = getch(); switch (tecla) {
case I;' : control-botones(0); break:
case 'e' : control-botones(l); break;
case '=' : control-botones(2); break;
case 'S' : control-botones(3); break;
case I?' : control-botones(4); break;
case '@' : control~botones(5); break;
case 'A' : control~botones(6); break;
case '6' : control-botones(7);
46
break;
1 break;
}
1
return(fin); 1; Pm*H***HC*H***H**H******************************************~*~H***~*~H*~*H~*~ I
r LLAMADA A TODAS LAS FUNCIONES PARÁMETROS DE TIEMPO Y POSICION *I . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1 .*
void in¡-parametros0 { int i;
ctl-boton = O;
ctl-rnovim = O;
for(i=O;i<4;i++) {
pos-h[i].ti-hour = O; pos-h[i].ti-min = O; pos_h[i].tjsec = O; pos-h[i].tjhund = O;
max-h[i].ti-hour = O; rnax-h[i].ti-rnin = O; rnax-h[i].ti-sec = O; max-h[i].ti-hund = O;
rnax-h[O].ti-sec = 11 ; /" tiempo de giro */ rnax-h[l].ti-sec = 4; P elevacion de brazo */ max_h[2].tjsec = 5; /* elevacion de muneca */ rnax_h[3].tjsec = 3; P cierre de pinzas *I
I
Í \ :
PARAMETROS DE TIEMPO MAXIM0 DE MOVIMIENTO. EN ESTE PUNTO SE PUEDEN MODIFICAR LOS VALORES PARA OBTENER UNA MEJOR EMULACION DEL MOVIMIENTO DEL GRAFICO CON EL BRAZOREAL.
bx[0]=105,by[0]=195, bx[l]=l05,by[l]=265, bx[2]= 155, by[2]= 1 95, bx[3]=155,by[3]=265, bx[4]=205,by[4]=195, bx[5]=205,by[5]=265,
47
bx[6]=255, by[6]=195, bm=255,bym=265;
outport(TARJE,OxFF); I"""*" BRAZO APAGADO --***** i
ma-x = 9x2-gxl ;
maxJ = gy2-gyl;
pcb = gy2; pct = gyl; pcr = g x 2 ; pcl = gxl;
a = (pcr-pcl) / ( l+ l ) ; b = pcl - a (-1); c = (pct-pcb) / (l+l); d = pCb - c (-1);
fr0m.x = 1 .O; fr0m.y = 0.0; from.2 = 0.0; at.x = 0.0; at.y = 0.0; at.z = 0.0; up.x = 0.0; up.y = 0.0; up.2 = 1.0;
angle = torad(60); t-from = 1 .O;
P POSICION INICIAL *I
I
vbase.1 = 3,.0; vbase.t = 0.0; vbase.f = 0.0;
v b r a z . 1 = 3.0; vbraz.t = 0.0; vbraz.f = 0.0;
mano.¡= 3.0; vman0.t = 0.0; wnan0.f = 0.0;
vertices = 4; pointarray[O].x= 1 .O; pointarray[O].y= 0.0; pointarray[O].z= 0.0;
*/
pointarray[l].x= 0.0; pointarray[l ].y= 0.0; pointarray[l].z= 0.0;
48
pointarray[2].x= 0.0; pointarray[2].y= 1 .O; pointarray[2].z= 0.0;
pointarray[3].x= 0.0; pointarray[3].y= 0.0; pointarray[3].z= 1 .O;
setat(); setfromo; seteye():
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 P INICIALIZA LOS PARÁMETROS
void inicializa() { clrscr(); in¡-parametros(); inigraficos0; pantalla(); setwritemode(3); setcolor( 15); draw-brazo(0);
I . . . . . . . . . . . . . . . . . . . . .
/* MANDA LA SEÑAL AL PUERTO (SI ES VALIDA) *I
void control() { int s-fin = O;
do {
/* control de movimiento */ ctl-mov();
I
/' control de botones */ s-fin = ctl-bot();
49
P control de simulac */ ctl-sim();
)while (!s-fin);
1;
50
............................................................................................. I r* FIN - CIERRA Y DETIENE TODO *I
void main() { inicializa(); control(); closegraph(); outport(TARJE,OxFF);
1; 2 2 5 8 3 2
51
CONSTRUCCION DE LA INTERFAZ CIRCUITO LOGIC0
ETAPA DE CONTROL MANUAL
La salida del puerto paralelo puede ir directamente al circuito lógico y posteriormente a los motores, sin embargo, debemos construir simultáneamente un circuito manual que permita por medio de botones controlar el brazo, esto implica que cuando el control manual esté inhabilitado, las salidas del puerto deben estar habilitadas y cuando el control manual esté habilitado, las salidas del puerto deben estar inhabilitadas.
Para este control se requiere una compuerta buffer de 3 estados ( 74LS240).
La figura anterior muestra una “compuerta buffer” de 3 estados, cuando la entrada de control “C” es alta, la compuerta se habilita y se comporta como un buffer normal, es decir, la salida “Y” es igual al valor binario de entrada “A”. Cuando la entrada decontrol es baja, la salida Y‘ es un circuito abierto, lo cual genera una impedancia alta (tercer estado) sin importar el valor de entrada “A”.
Una característica importante de estas compuertas es que el retardo de habilitación de salida es más largo que el retardo de inhabilitación de salida, esto es importante porque sin un circuito de control habilita una compuerta e inhabilita otra al mismo tiempo, la compuerta inhabilitada entra en estado de alta impedancia antes que la otra compuerta se habilita. Esto elimina la situación de que ambas compuertas estén activas al mismo tiempo.
Hay una pequeña corriente de fuga asociada con la condición de alta impedancia, sin embargo, esta corriente es tan pequeña que hasta 100 compuertas de 3 estados pueden conectarse juntas para formar un bus de línea común.
El siguiente circuito muestra como pueden ser utilizadas esta compuertas para tener un control manual simultáneamente a la salida del puerto paralelo, se utiliza un bus de compuertas en línea para el puerto paralelo y otro para el control manual por botones.
52
Boton de 1 f
C o n t r o l I manual
350 ohms
Cuando el botón enable del control manual no es presionado, la entrada de control está en baja para toda la línea de compuertas de los botones, por tanto son inhabilitadas, y la salida de estas compuertas tendrán )a condición de alta impedancia o circuito abierto independientemente de que botón de entrada sea presionado en ese momento,.simultáneamente, el inversor conectado al botón enable hacia la línea de compuertas del puerto paralelo genera una entrada de control alta, habilitando todas las compuertas del puerto cuyo valor de salida será igual al valor de entrada para cada una.
Cuando el botón enable del control manual es presionado, la entrada de control está en alta para toda la línea de compuertas de los botones, por tanto son habilitadas, y la salida de estas compuertas tendrán el valor binario de los botones,.simultáneamente, el inversor conectado al botón enable hacia la línea de compuertas del puerto paralelo genera una entrada de control baja y el consecuente circuito abierto o de alta impedancia bloqueando cualquier valor de entrada del puerto paralelo.
Como ya se explicó antes, podemos hacer una línea de hasta 100, para este caso se requieren 8 para controlar todos los movimientos del brazo de mecánico.
53
ETAPA DE CONTROL DE GIRO DE LOS MOTORES
Independientemente de que la información binaria venga del puerto paralelo o del control manual, tenemos dos valores para el movimiento de cada motor, en este caso la lógica seleccionada se especifica en las siguiente tabla de verdad:
1 Entradas I D O Dl
Salidas Acción A S ¡
l o [ o j o ! O \ l I O
0 ; Detenido 1 1 Giro Derecha p ~ o j l j 0 ' Giro Izquierda
; I ! i l o i o Detenido I
Las ecuaciones de estado para esta tabla son:
A=DODI
B = E D I
Circuito lógico:
Este circuito puede ser simplificado un poco con una compuerta XOR.
Y x-
Con esta compuerta podemos construir un circuito compatible de la siguiente manera:
54
Circuito simplificado
DO
D l
I Entradas 1 Salidas I I D Dl A B *
Con este circuito tenemos el control de giro de cada motor, la siguiente etapa de construcción deberá de suministrar la corriente necesaria a los motores para su operación.
55
ETAPA DE CONTROL DE POTENCIA
Para suministrar la potencia y el giro adecuado en función de la etapa anterior, utilizaremos transistores de potencia tipo TIP31A cuyas características de operación son las siguientes:
56
Con cuatro transistores TIP 31A podemos construir un circuito que suministre la potencia necesaria al motor para su funcionamiento con el sentido de giro adecuado.
El diagrama siguiente, es un circuito de conmutación por transistores de corte y saturación.
Los transistores de base común trabajan en forma conmutada para generar la potencia y el sentido de la corriente necesaria para el funcionamiento del motor, “do” tiene base común con T I y T2 y “d l ” con T3 y T4. Cuando dO = 1 y d l = O según la señal enviada por el circuito lógico de la etapa anterior, T1 y T2 entran en saturación y T3 y T4 en corte, por tanto, la corriente circula según el siguiente esquema:
r
57
Análogamente, Cuando dO = O y d l = 1, T I y T2 entran en corte y T3 y T4 conducen corriente en sentido contrario, modificando el sentido de giro del motor según el siguiente esquema:
AI término del análisis del circuito, se realizó el montaje en una “protoboard” para revisar el funcionamiento antes de soldar el circuito final en una tableta
58
DIAGRAMA GENERAL Las etapas descritas con anterioridad son descripciones parciales del circuito final ya que solucionan el problema considerando solo 2 bits que son requeridos para el control de un motor. El siguiente diagrama reune las etapas para el control de los cuatro motores utilizando los ocho bits de salida del puerto paralelo.
I Puerto paralelo
'1 Boton de
Con t rol manual
q
MATERIAL PARA SU CONSTRUCCIóN: 1
1 74LS240 BUFFER 2 AND 74LSOO 2 XOR 74LsS6 1 NOT 74LS04
16 TRNSISTORES TIP 31A
59
MANUAL DE USUARIO
Interfuz Dentro de una caja de metal se montó el circuito lógico, de ella, salen 4 pares de líneas de colores para controlar los motores del brazo mecánico, 8 botones fueron montados sobre la caja para accionar los movimientos del brazo en forma manual, y uno adicional habilita esta función manual. El circuito posee una cable con conector para puerto paralelo para establecer comunicación con el sistema.
t f
J Conexión:
El par blanco se conecta al eje de la base del brazo (giro izq. y der.). El par amarillo se conecta al tronco del brazo (elevación del brazo) El par negro se conecta al eje de la muñeca del brazo (mov. de muñeca). El par azul se conecta al eje de las pinzas del brazo (mov. de pinzas).
El par trenzado de naranja y negro se conecta a una fuente de poder de 5v a 1.5amp y tierra respectivamente.
El cable gris se conecta al puerto paralelo de la computadora
60
Operación: 2 2 5 8 9 2
0 AI presionar el botón lateral, se habilita el movimiento manual, y se puede mover el brazo mecánico, presionando uno a la vez, los botones que se encuentran en la parte superior
m Si el brazo se mueve en sentido contrario del movimiento deseado, se deben intercambiar los cables de este movimiento,
0 Mientras se mantiene presionado el botón que habilita el movimiento manual, cualquier orden que se envié por la computadora, será ignorado por el robot.
0 Cuando no se presiona el botón lateral de control manual, el circuito espera la señal de movimiento generada por el programa y enviada por el puerto paralelo.
61
Software Instalación:
El programa se encuentra como archivo ejecutable con nombre "MECANICO.EXE", para ejecutarlo solo basta teclear MECANICO desde la dirección donde se encuentra el archivo.
Los requerimientos mínimos del sistema para utilizar este programa son los siguientes:
Computadora 486 DX 8 MB de memoria RAM Monitor VGA color
Operación:
La pantalla de control del brazo se muestra en la siguiente figura:
0 La pantalla se divide en dos partes, de lado izquierdo se encuentran los botones de activación de movimiento y de lado derecho se localiza el grafico de emulación.
0 Antes de iniciar el movimiento, el brazo real se debe posicionar en la forma en que se encuentra el gráfico en la pantalla.
los botones de la pantalla se activan con las teclas de funciones del teclado de la computadora.
62
0 Las funciones realizar los siguientes movimientos:
- F1: gira el brazo a la derecha. - F2: gira el brazo a la izquierda. - F3: sube el brazo. - F4: baja el brazo - F5: sube la pinza - F6: baja la pinza - F7: abre la pinza - F8: cierra la pinza
O AI momento de accionar una tecla, se moverá la simulación del brazo mecánico en la pantalla y mandará la señal necesaria a la interfaz para mover el brazo mecánico real..
0 Si el brazo no se mueve, esto debe a que este movimiento llegó a su ángulo máximo o máxima posición.
O Para detener el movimiento del gráfico se debe presionar nuevamente la tecla con la cual se inició. Cuando el gráfico llega a su máxima posición, automáticamente se detiene.
63
l3lBLlOGRAFlA
Q TURBO C/C++ MANUAL DE REFERENCIA Schild Herbert Ed. Mc. Graw Hill
0 EL LENGUAJE DE PROGMMACIÓN C++ Kemighan W. Brian 2da. ED. Ed. Prentice Hall Hispanoamericana
*:* PROGRAMACIÓN DE GMFICOS EN TURBO C++ Euell Ben '
Ed. Addison - Wesley / Diaz de Santos
0:. ROSdTlCA PRACTICA - " TECNOLOGíA Y APLICACIONES " Usastegui Angulo José Ma. Madrid 4a ED. - 1996 Ed. Paraninfo
0 GUíA FACIL DE ROBÓTlCA Usastegui Angulo José Ma. Madri Ed. Paraninfo
THE TTL DATA BOOK - FOR DESIGN ENGINEERS Texas Instrument Incorporated Second Edition
03 POWER PRODUCTS DATA BOOK - TRANSISTORS Texas Instrument - 1990
Q DISENO DIGITAL Mano Moms M. Ed. Prentice Hall Hispanoamericana
0:. MICROELECTRdNICA CIRCUITOS Y DISPOSITIVOS Horenstein N. Mark 2da. ED. Ed. Prentice Hall Hispanoamericana
64