manual de operaciones robot delta

Universidad Autónoma de Querétaro Facultad de Ingeniería

Aguilar Hdez. Gustavo Arana Ruiz. Diego Torres Ortega. Carlos Alberto

DISEÑO E IMPLEMETACIÓN DE HARDWARE.

Antes de comenzar, es necesario entender como está estructurado el sistema. La

arquitectura de este sistema en particular, se aprecia en el siguiente diagrama.

Figura 1. Arquitectura del sistema.

DISEÑO MECÁNICO.

El brazo delta consta de 3 Actuadores dispersos uno del otro cada 120 grados,

cada uno actúa independientemente, pero es necesario recordar que un

mecanismo paralelo. Es sumamente importante recordar esto en el momento de

obtener los cálculos. Está estructurado de brazos, vínculos l (links) y uniones j

(joints) tanto esféricas E como rotatorias R. La arquitectura de un brazo delta se

puede apreciar en la Figura 2.

BRAZO DELTA SERVO CONTROL PC Encoder Halls

Alimentación

Comunicación RS232

DISEÑO MECÁNICO DISEÑO ELECTRÓNICO



Figura 2. Arquitectura y distribución geométrica de un brazo delta.

Materiales utilizados:

Material Lugar de uso.

Perfil cuadrados de

Aluminio (3/4).

Brazos

Canales de Aluminio

(9mm).

links

Ejes de acero. Uniones

Cojinetes para los ejes. Uniones: para soportar y disminuir fricción de los ejes.

Anillos de retención para

los ejes.

Uniones: para restringir que el eje se moviera de su posición

preestablecida.

PTR (2 in). Estructura.

Madera Estructura: Soporte de los Actuadores. (servomotores); Soporte para

la interfaz de servocontrol.

Gomas de plástico. Utilizadas como coples para la unión Motor-Brazo.

DISEÑO ELECTRÓNICO.

Alimentación.



Para alimentar la etapa de control, se utiliza una fuente de computadora, así como

también para alimentar la tarjeta que contiene al DAC y el Amplificador, pues es

requerida una fuente simétrica de 12VDC. Para la alimentación de los servodrivers

(Este valor depende del fabricante) se ocupa una fuente mayor a 20VDC capaz

de suministrar una corriente mayor a 6Amp.

Figura 3. Fuente Simétrica de computadora y fuente de 24VDC a 10 A.

Otro circuito se utiliza como receptor de la alimentación proveniente de las dos

fuentes, para así después ser distribuida la corriente hacia todo el sistema. Se

muestra a continuación el circuito mencionado.

Figura 4. Circuito de distribución de carga.

Motores.

Los motores utilizados para esta aplicación son los que se muestran en la foto.

Motores sin escobillas de DC.

Fuente 24VDC 10 Amperes.

Fuente CPU. 5V, 12V, -12V



Y le corresponden estas características:

NOTA: los datos fueron obtenidos a partir del análisis de señales provenientes del

motor, puesto que no existe una hoja de datos o especificaciones en internet para

este motor.

Para la distribución de corriente de los motores se hizo un circuito que actúa como

receptor de las 3 fases y la alimentación del encoder, y como emisor de las

señales provenientes del encoder y de los sensores de efecto hall. A continuación

se muestra la imagen de cómo fueron distribuidas las señales:



Como se puede notar en la imagen, los conectores P1, P2 Y P3 tienen su

referencia a tierra a un costado del conector P1F, P2F y P3F que corresponden a

cada uno de los motores donde se unen las fases que son regidas por el

servodriver.

TIP: siempre que se requiera conectar los motores, identificar la tierra en su

conector para posteriormente conectarlo.

Servocontrol.

La Electrónica del servocontrol se divide en 2 partes principales:

1. Etapa de control.

2. Etapa de servo amplificación.

Etapa de control. Se utilizó un PIC18f2331 por sus características especiales

(consultar datasheet), cabe mencionar que no se explotó el potencial del

micocontrolador, pero la tarjeta está diseñada para que en futuras aplicaciones, el

servodriver sea sustituido por el PIC.

El PIC recibe varios comandos desde la PC por los cuales ejecuta distintas

acciones. Más adelante se describe el diseño del software. A la salida de este,

envía por el puerto B una señal digital de 8 bits para posteriormente ser convertida

a analógica en la etapa de servo-amplificación.

A cada uno de los PIC se les dio un identificador en especifico (M1, M2 y M3) para

poder ser reconocidos por el software y para que ellos puedan reconocer a quien

Hacia el servocontrol

Hacia los motores



de los 3 se le ha hecho la petición de ejecución. En la imagen(Figura 2) se

muestra a quien corresponde cada etiqueta.

A continuación se detalla la tarjeta que contiene al PIC.

Nota: El PIC M3 posee una diferencia entre los demás, pues por defectos de

construcción la salida serial(Pines RX y TX) fueron invertidos, es decir, que si en

M1 y M2, RX es el pin mencionado en la figura, para M3 es TX.

C3 es una salida digital: INDEX para el Modulo de cuadratura (QEI). Pues la

arquitectura del motor no los contiene.

C5 es una salida digital: habilitador para poder enviar por serial el dato, puesto que

la comunicación es con 3 PICs, para que no se mezclen las señales.

M3 M2 M1

PIC

Alimentación

ICSP

RESET

LED PIN_C0

LED indicador de alimentación

QEI



Etapa de servo amplificación. Esta contiene todos los servodrivers y la señal de

referencia proveniente de la etapa de control. Es importante, recurrir al datasheet

de cada uno de los servodrivers, para lograr comprender las conexiones y su

configuración (En caso de no tener conectado nada o no tener conectores,

recurrir directamente al datasheet y crear una nueva conexión como es debido)

El microcontrolador contenido en la Etapa de Control no posee un DAC interno,

por lo que es necesario utilizar uno externo para vincular el control con el

servodriver. Es importante mencionar que la señal aun debe ser amplificada, es

por eso que un amplificador debe cumplir esta función. A continuación se muestra

el circuito utilizado:

Comunicación Serial.

DAC

Alim

en

tación

Amplificador

REFERENCIA

servodriver



Para la comunicación serial fue requerido un circuito lógico para no cruzar la

comunicación entre dispositivos. A continuación se muestra el circuito referido y

como debe ser conectado con la tarjeta que contiene al MAX232:

Puesto que no es muy clara la imagen en la parte rodeada por el círculo, a

continuación se ejemplifica las conexiones seriales para el PIC.

RX TX GND /PC RX TX /MAX

RX TX /M1

RX TX /M2

RX TX /M3



PROGRAMA DE LOS CONTROLADORES. PID DE LOS MICROS

El programa de control para los motores se realizo mediante el uso de MICROCONTROLADORES,

específicamente el PIC 18F2331 de MICROCHIP, el cual tiene con un módulo de cuadratura que

nos permite monitorear las cuentas del encoder y de esta manera realizar un lazo de control. A

continuación se colocara el código fuente del controlador y se explicara línea a línea el

funcionamiento del programa.

#include <18f2331.h> #include <stdlib.h> #fuses HS, NOWDT, NOBROWNOUT, NOPROTECT, NODEBUG #use delay (clock = 8MHz) #use RS232 (baud = 38400, rcv=PIN_C7, xmit=PIN_C6, bits=8, PARITY=N, stop=1)

// Líneas de cabecera se establecen los fusibles principales, un oscilador externo de 8 MHz y el puerto UART con la que cuenta el microcontrolador

#define buf_length 32 // Se define un tamaño de buffer. int buf[buf_length], str[buf_length]; int str_flag = 0, i=0, y=0; int ref_flag = 0; // Se definen variables principales para manejar los estados del programa. //###################### COMANDOS ############################################### int dist [6] = {'D','I','S','T','.', 0}; int reset [5] = {'R','S','T','.', 0}; int move [5] = {'M','O','V','.', 0}; int pos [5] = {'P','O','S','.', 0}; int esc [5] = {'E','S','C','.', 0}; int motor1 [4] = {'M','1','.', 0}; // Se definen los comandos a los cuales el microcontrolador responde al ser recibidos en su puerto UART.



//################## VARIABLES PID ############################################### int16 cuentas = 0; //lectura de cuentas int16 control = 0; //valor int16 posRef = 0; //referencia de posicion float rT, eT, yT, uT, iT_1, eT_1; //variables de ecuaciones float max, min; //límites máximo y mínimo de control. float Kp, Ki, Kd; //constantes del PID float senalControl = 0; float pT = 0, qT = 0; float pT_1 = 0.0; int id = 0; //identificador del PIC/MOTOR int1 flagPID = 0; // Se definen las variables que utiliza el PID, y se inicializan a 0 algunas de ellas. //@@@@@@@@@@@@@@@ INTERRUPCION SERIAL @@@@@@@@@@@@@@@@@@@ #INT_RDA void rcv_data() { buf[i]=getchar(); if (buf[i] == '.') // Ingresa a esta opción hasta que encuentra un “.” { str_flag = 1; // Activa la bandera que indica que recibió una instrucción. buf[i+1] = 0; strcpy (str,buf); // Copia el buffer a la cadena de comparación. for(y=0; y< buf_length ;y++) // Coloca los valores del buffer en 0 buf[y] = 0; i = 0; } else{ i++; } }

// Función de la interrupción del UART, captura el todos los caracteres que se reciban por el puerto y los va guardando en el buffer hasta que encuentre un “.” (punto), al encontrar este ultimo carácter copia el buffer sobre la cadena de comparación.



#INT_TIMER2 // Interrupción del TIMER 2 void intTimer(){ if(flagPID){ cuentas = qei_get_count(QEI_GET_POSITION_COUNT); if (cuentas > 32767) senalControl = (float)(-1)*(65538 - cuentas); else senalControl = (float)cuentas; yT = senalControl; // Se muestrea el numero de cuentas rT = (float)posRef; eT = rT - yT; //Cálculo error pT = (0.001*eT) + pT_1; //Cálculo del término integral qT = Kd*((eT-eT_1)/0.001); //Cálculo del término derivativo uT = (Ki*pT) + (Kp*eT) + qT; //Cálculo de la salida PID if (uT > max) { //Salida PID si es mayor que el MAX uT = max; }else if (uT < min){ //Salida PID si es menor que el MIN uT = min; } control = (int16)uT; //Transferencia de salida PID output_b (control); pT_1 = pT; //Guardar variables eT_1 = eT; if (eT <= 10 && eT >= -10 && ref_flag <= 30){ output_high(PIN_C5); printf(" M%d OK \r\n", id); output_low(PIN_C5); ref_flag ++; } } output_toggle (PIN_C0); // Sirve para observar en el osciloscopio la frecuencia de muestreo. }

// Dentro de esta interrupción se realizan los cálculos del PID, el cual manda la señal de control a la etapa de servo amplificación.

//########################################################################## MAIN



void main() { //--------------------- Define states on State Machine STM ----------------- enum state{STM,STMST0, STMST1,STMST2,STMST3,STMST4}currentState; // Se define el enumerador setup_qei(QEI_MODE_X4|QEI_VELOCITY_MODE_DISABLED,QEI_FILTER_ENABLE_INDX ,QEI_FORWARD); setup_timer_2(T2_DIV_BY_4,125,2); // Se configura el módulo de cuadratura y el TIMER 2 enable_interrupts (INT_RDA); enable_interrupts (INT_TIMER2); enable_interrupts (GLOBAL); // Se inicializan las interrupciones set_timer2(0); // Se inicia el conteo del TIMER 2 en 0

// Valores predeterminados para cada motor min = 0.0; // Valor mínimo de la señal de control max = 255.0; // Valor máximo de la señal de control posRef = 500.0; // Referencia de posición a alcanzar (en cuentas) iT_1 = 0.0; eT_1 = 0.0; Kp = 1.5; Ki = 3.0; Kd = 0.0005; // Valores de las constantes del PID.

Es muy importante sintonizar estas variables para cada uno de los motores, ya que cada motor es una planta diferente y los valores de estas constantes deben ser diferentes para cada motor. TIPs: 1. Para sintonizar las constantes del PID es recomendable poner Ki y Kd igual a 0

y comenzar con un valor de Kp = 1, observar la reacción del control y con esto comenzar a variar la Kp hasta observar que el motor llegue a una posición en la cual no esté vibrando y que al tratar de mover la flecha del motor se sienta una fuerza igual que no te permita mover la flecha con facilidad y que esta regrese a la posición a la cual había llegado anteriormente, no importa que exista un error muy grande entre la posición de la flecha y la referencia.



2. Al tener buenos resultados con el primer paso ya podemos comenzar a aumentar el valor de Ki, recuerda que esta constante disminuye el error pero nos aumenta el sobrepaso. Lo recomendable en este punto es comenzar a darle valores bajos a Ki y observar los efectos sobre nuestro controlador. El valor más recomendable para Ki es cuando el error sea 0 y el sobrepaso que muestre no sea mucho, es decir, que casi no vibre el motor para llegar a la referencia.

3. Al tener ya una Kp y una Ki adecuadas se recomienda colocar una Kd, recuerda que esta constante reduce el sobre paso, pero introduce ruido al sistema. Es por eso que es valor recomendado de Kd es un en el cual el sobre paso sea nulo y que cuando llegue a la referencia y/o se le aplique una carga a la flecha del motor esta no comience a vibrar.

output_high(PIN_C0); output_low(PIN_C3); output_b (128); // Salida hacia el DAC, recuerda que para el DAC un 128 en decimal ó un 0x70,

0x0F es un 0 a la salida del amplificador (Revisa la hoja de datos del DAC 0800) //!//--------------------- Define events on State Machine STM ----------------- //! currentState= STM; while(TRUE) { switch(currentState) { //############################# STATE Master ####################################### case STM: //! //motor 1 if (strcmp(str,motor1)==0){ id=1; output_high(PIN_C5); printf("\n\rMOTOR%d \n\r", id); output_low(PIN_C5); currentState=STMST0; }

// Primer estado del programa, espera a recibir su código para ingresar al resto del programa, en este caso su código es M1

break;



//############################# STATE 0 ####################################### case STMST0: if(str_flag) { if (strcmp(str,pos)==0){ currentState= STMST1; }else if (strcmp(str,move)==0){ flagPID=1; }else if (strcmp(str,dist)==0){ currentState= STMST3; }else if (strcmp(str,reset)==0){ currentState= STMST2; }else if(strcmp(str,esc)==0){ output_high(PIN_C5); printf("end \n\r"); output_low(PIN_C5); currentState= STM; } str=""; str_flag=0; } // En este estado el controlador espera a recibir una orden para ejecutar. break; //############################# STATE 1 ####################################### case STMST1: //POSICION....Obtiene el numero de cuentas del QEI cuentas = qei_get_count(QEI_GET_POSITION_COUNT); // Regresa el número de cuentas que se ha

movido // el motor. Este valor es un valor obsoluto.

output_high(PIN_C5); printf("C%d = %ld \r\n",id, cuentas); output_low(PIN_C5); currentState= STMST0; // Manda por el puerto UART a la PC la posición, en cuentas, de la flecha del motor. break;



//############################# STATE 2 ####################################### case STMST2: //RESET.

qei_set_count(0); // El valor del módulo de cuadratura es igual a lo que se escriba con //esta función.

output_high(PIN_C5); printf(" M%d Listo \n\r", id); output_low(PIN_C5); currentState=STMST0; // Resetea el módulo de cuadratura, igualando la posición del encoder a 0 cuentas. break; //############################# STATE 3 ####################################### case STMST3: //DIST. output_high(PIN_C5); printf(" M%d REF: \r\n", id); output_low(PIN_C5); currentState=STMST4; // Indica que el siguiente dato recibido por el puerto UART es la nueva referencia. break; //############################# STATE 4 ####################################### case STMST4: // if(str_flag) { posRef = (int16)(atof(str)); output_high(PIN_C5); printf("Guardado %lu \r\n", posRef); output_low(PIN_C5); ref_flag = 0; currentState=STMST0; } // Iguala el valor recibido en el puerto UART a la referencia de posición. break; //########################## Default ########################################## default: currentState=STMST0; // El estado de dafeault nos manda al estado 0. break; } } } // Termina el programa.



Es recomendable para el mejor entendimiento del programa tener nociones del concepto de Máquina de Estados y de estudiar en la ayuda del compilador CCS las funciones que no sean entendibles para el usuario.



INTERFAZ GRÁFICA PARA EL CONTROL DEL ROBOT DELTA

Se deberá desarrollar un programa que cumpla las necesidades de realizar los

cálculos de θ1 de cada uno de los motores para a partir de este dato saber cuantos

grados tienen que rotar nuestros motores.

Para poder hacer el cálculo de θ 1 se necesitan calcular primero los parámetros de

θ 2 y θ 3, además de ingresar datos importantes de la arquitectura del robot como lo

son las siguientes variables:

h = es la distancia del centro de la plataforma del “end efector” a un lado del

mismo.

r = es la distancia del centro de la plataforma de la base a un lado de la misma.

a = Es la longitud de la articulación de menor tamaño.

b = Es la longitud de la articulación de mayor tamaño.

También se necesita que el programa calcule las velocidades lineales; y que todos

estos datos sean enviados a través de un puerto de la computadora, para este

caso a través del puerto serie.

Cálculos

Para poder determinar cuáles serán los valores de nuestros ángulos y saber la

posición del “end efector” necesitamos realizar y resolver una serie de ecuaciones.

Primero es necesario determinar los vectores de nuestro robot como se muestra a

continuación:



AiBi BiCi OP PCi OAi

Los cuales contienen los siguientes datos:

acos1i

0

asin1i

bsin3icos1i 2i

bcos3i

bsin3isin1i 2i

cos i sin i 0

sin i cos i 0

0 0 1

Px

Py

Pz

h r

0

0

Despejando de los vectores anteriores, para poder determinar la posición final del

“end efector” se encuentra el siguiente resultado:

Cx Px cosi Pysini h r

Cy Px sini Pycosi

Cz Pz

Posteriormente de determinar el resultado de la posición podemos hacer el

despeje de ecuaciones y la relación entre ellas para encontrar las ecuaciones que

rigen los ángulos en cada uno de los brazos del robot delta.

1i sin1 k2Cxak1Cz

k22ak12

2i cos1 Cx2Cy2Cz2a2b2

2ab sin3i

3i cos1 Px siniPycosi

b



Plataforma de programación:

Para poder desarrollar esta interfaz fue necesario hacer la descargar y la

instalación de Microsoft Visual C#, el cual puede ser conseguido desde internet de

forma gratuita, en la página oficial de Microsoft.

Al momento de la descarga se puede realizar la instalación del mismo, una vez

seleccionado la ruta de instalación. Para que se pueda tener acceso a las

actualizaciones del programa y a mejoras del Visual C# es necesario registrarlo,

esto se puede llevar a cabo al termino de la instalación, pues aparecerá un

mensaje si desea registrar el producto. El producto puede ser registrado de forma

gratuita en donde pedirá una cuenta de e-mail y un password. Al término de la

instalación y el registro del producto se puede empezar a programar de manera

dinámica y muy grafico-visual.

Se ha desarrollado el programa en este ambiente por las facilidades de

programación que otorga, ya que nos ofrece una programación orientada a

objetos, y para poder programar en este ambiente es necesario conocer algunos

conceptos como lo es las clases, los objetos, la herencia etc.



Explicación del programa desarrollado: VENTANA PRINCIPAL FORM1:

Las siguientes librerías son indispensables en el programa pues dan las

herramientas necesarias para que el programa se lleve a cabo según lo planeado.

//Es indispensable para las aplicaciones de sistema

using System;

// Librería para objetos genéricos

using System.Collections.Generic;

// Nos habilita los componentes

using System.ComponentModel;

// Trabaja para procesar datos

using System.Data;

// Libreria que habilita las herramientas de dibujo

using System.Drawing;

//Genera el dll del archivo

using System.Linq;

//Muestra el texto

using System.Text;

//Es fundamental par a las opciones de objetos que se quieran agregar

using System.Windows.Forms;

//Se habilitan las entradas y salidas de los puertos de la computadora

using System.IO.Ports;



//Libreria para hacer dibujos en 2D

using System.Drawing.Drawing2D;

El “namespace” es el espacio en cual se desarrolla cualquier código que quiera

ser agregado al programa para este algoritmo se le ha dado el nombre de

Robot_Delta.

namespace Robot_Delta

{

Esta es mi clase principal en donde prodrémos ejecutar las funciones que sean

necesarias para que el programa funcione adecuadamente

public partial class Form1 : Form

{

Abajo se tiene la declaración de cada una de las variables que necesitamos para

poder realizar los cálculos.

private String t1,t2,t3; private String px, py, pz; private Double tt1, tt2, tt3; private Double tt12, tt22, tt32; private Double tt13, tt23, tt33; private Double t11, t12, t13; private Double ppx, ppy, ppz; private Double cx, cy, cz; private Double cx2, cy2, cz2; private Double cx3, cy3, cz3; private Double k,k1, k2; private Double jx1, jy1, jz1; private Double jx2, jy2, jz2; private Double jx3, jy3, jz3; private Double jq1, jq2, jq3; private Double jqq1, jqq2, jqq3; private Double vp1, vp2, vp3; private Double teta1, teta2, teta3; private Pen myPen; private String p; private String r; private String o;



private String b; private String ha; private String v1; private String v2; private String v3; private String cuenta; private float hh; private float rr; private float o1= 0; private double o2= (120 * Math.PI)/180; private double o3 = (240 * Math.PI) / 180; private float bb; private float aa; private float cuentas; private float vv1; private float vv2; private float vv3;

Con la siguiente function se inicializa nuestra interfaz.

Observar figura 1.

public Form1() { InitializeComponent(); }



Figura 1.

El programa cuenta con dos funciones adicionales ya que los cálculos para

posicionar el “end efector” los podemos hacer a través de la cinemática directa o

inversa, la cual es posible seleccionarla a través de una función dinámica llama

radio button donde solo es necesario dar un click en cualquiera de las dos

opciones para que sea posible configurar respectivos parámetros de información

que van a ser diferentes para ambos casos. Observar figura 2 y 3.

private void rbdirecta_CheckedChanged(object sender, EventArgs e) { label2.Visible = true; label4.Visible = true; label5.Visible = true; label6.Visible = true; tbt1.Visible = true; tbt2.Visible = true; tbt3.Visible = true; label3.Visible = false; label7.Visible = false;



label8.Visible = false; label9.Visible = false; tbx.Visible = false; tby.Visible = false; tbz.Visible = false; }

Figura 2.

private void rbinversa_CheckedChanged(object sender, EventArgs e) { label2.Visible = false; label4.Visible = false; label5.Visible = false; label6.Visible = false; tbt1.Visible = false; tbt2.Visible = false; tbt3.Visible = false; label3.Visible = true; label7.Visible = true; label8.Visible = true; label9.Visible = true; tbx.Visible = true; tby.Visible = true; tbz.Visible = true; }



Figura 3.

La siguiente función viene dada por el formulario de un botón en el cual realiza

abre otra ventana para poder configurar parámetros de un robot en especifico.

Observar figura 4.

private void button1_Click(object sender, EventArgs e) {

Figura 4.

En las siguientes dos líneas de código se define a un objeto “conf” que va tener las

propiedades y atributos que mi función “Configuracion” en esta función

configuración se puede modificar las características del robot. Observar figura 5.

Configuracion conf = new Configuracion(); DialogResult resulta = conf.ShowDialog();



Figura 5.

Una vez que se de click en el botón aceptar todos las especificaciones que

hayamos puesto en la ventana configuración se van igualar a otras varibales con

el fin de hacer los cálculos desde esta ventana llamada “Form1” que va ser

nuestra ventana principal.

if (resulta == DialogResult.OK) { p=conf.H; r=conf.R; o = conf.O; b=conf.B; ha=conf.HA; cuenta = conf.Cuentas; hh = float.Parse(p); rr = float.Parse(r); bb = float.Parse(b); aa = float.Parse(ha); //cuentas = float.Parse(cuenta); } conf.Dispose(); conf = null;



}

Una vez que se abre la ventana de “Form1” va cargar desde el principio todos lo

puerto que encuentre en la computadora y lo mostrará en un combo box, para eso

se crea la siguiente función. Observar figura 6.

private void Form1_Load(object sender, EventArgs e) { foreach (String s in SerialPort.GetPortNames()) { cbpuertos.Items.Add(s); } }

Figura 6.

Para poder realizar los cálculos que necesitamos para encontrar todas las ϴ1 de

cada uno de los motores así como su velocidad deseada. Es importante

mencionar que se necesita llenar la caja de texto de X, Y, Z, V1, V2, V3 así como

agregar elementos a la configuración del robot, sin estos datos posiblemente nos

arroje un error que el programa lo interpreta como datos insuficientes.

private void button2_Click(object sender, EventArgs e)

{



Dependiendo del tipo de cálculo que se requiera en este botón se reconocerá si se

ha seleccionado cinemática inversa o directa a través de la siguiente instrucción:

if (rbdirecta.Checked == true)

{

//Variables para Cinemática Directa

t1 = tbt1.Text; t2 = tbt2.Text; t3 = tbt3.Text; tt1 = float.Parse(t1); tt2 = float.Parse(t2); tt3 = float.Parse(t3);

Los siguientes cálculos son indispensables para poder saber el punto final del “end

efector”.

Cx Px cosi Pysini h r

Cy Px sini Pycosi

Cz Pz

cx = (bb * Math.Sin(tt3)*Math.Cos(tt1+tt2)+aa*Math.Cos(tt1)-bb*Math.Cos(tt3)*Math.Sin(o1)-hh+rr)/(Math.Cos(o1)*Math.Pow(Math.Sin(o1),2)); cy=(bb*Math.Cos(tt3)+cx*Math.Sin(o1))/(Math.Cos(o1)); cz = bb * Math.Sin(tt3) * Math.Sin(tt1 + tt2) + aa * Math.Sin(tt1);

Y el resultado obtenido lo mostramos en una caja de texto

txtr.Clear(); txtr.Text = "Seleccionado Cinemática directa" + "\r\n\r\nResultado: " + "\r\nCx= " + cx + "\r\nCy= "+cy+"\r\nCz= "+cz; } if (rbinversa.Checked == true) {

//Variables para Cinemática Inversa

px = tbx.Text; py = tby.Text; pz = tbz.Text; v1 = tbv1.Text; v2 = tbv2.Text;



v3 = tbv3.Text; ppx = double.Parse(px); ppy = double.Parse(py); ppz = double.Parse(pz); vv1 = float.Parse(v1); vv2 = float.Parse(v2); vv3 = float.Parse(v3); float ytr; double s1; double c1; double st3; double aux3; double g1; //CALCULO PARA 01 DEL MOTOR 1 cx=ppx*Math.Cos(o1)+ppy*Math.Sin(o1)+hh-rr; cy=-ppx*Math.Sin(o1)+ppy*Math.Cos(o1); cz = ppz;

Para la cinemática inversa es necesario encontrar los ángulos, que están inmersos

en las ecuaciones para el control de posición de los brazos.

s1= Math.Sin(o1); c1 = Math.Cos(o1);

Se determina el ángulo ϴ3:

3i cos1 Px siniPycosi

b

tt3 =Math.Acos(((-ppx * s1)+(ppy * c1))/ (bb)); st3 = Math.Sin(tt3);


2i cos1 Cx2Cy2Cz2a2b2

2ab sin3i

g1=(2 * aa * bb * st3); k = (Math.Pow(cx, 2) + Math.Pow(cy, 2) + Math.Pow(cz, 2) - Math.Pow(aa, 2) - Math.Pow(bb, 2))/ g1; tt2 = Math.Acos(k);

Variables auxiliares para el cálculo de ϴ1:



k1=bb*Math.Sin(tt3)*Math.Cos(tt2); k2=bb*Math.Sin(tt3)*Math.Sin(tt2);


1i sin1 k2Cxak1Cz

k22ak12

tt1 = Math.Asin((k2*cx-(aa+k1)*cz)/(-Math.Pow(k2,2)-Math.Pow(aa+k1,2))); t11 = (tt1*180)/Math.PI;

De la misma manera ser realiza para el cálculo de la posición y del ángulo para los

otros dos brazos.

// CALCULO PARA 01 DEL MOTOR 2 cx2 = ppx * Math.Cos(o2) + ppy * Math.Sin(o2) + hh - rr; cy2 = -ppx * Math.Sin(o2) + ppy * Math.Cos(o2); cz2 = ppz; s1 = Math.Sin(o2); c1 = Math.Cos(o2); tt32 = Math.Acos(((-ppx * s1) + (ppy * c1)) / (bb)); st3 = Math.Sin(tt32); g1 = (2 * aa * bb * st3); k = (Math.Pow(cx2, 2) + Math.Pow(cy2, 2) + Math.Pow(cz2, 2) - Math.Pow(aa, 2) - Math.Pow(bb, 2)) / g1; tt22 = Math.Acos(k); k1 = bb * Math.Sin(tt32) * Math.Cos(tt22); k2 = bb * Math.Sin(tt32) * Math.Sin(tt22); tt12 = Math.Asin((k2 * cx2 - (aa + k1) * cz2) / (-Math.Pow(k2, 2) - Math.Pow(aa + k1, 2))); t12 = (tt12 * 180) / Math.PI; // CALCULO PARA 01 DEL MOTOR 3 cx3 = ppx * Math.Cos(o3) + ppy * Math.Sin(o3) + hh - rr; cy3 = -ppx * Math.Sin(o3) + ppy * Math.Cos(o3); cz3 = ppz; s1 = Math.Sin(o3); c1 = Math.Cos(o3); tt33 = Math.Acos(((-ppx * s1) + (ppy * c1)) / (bb));



st3 = Math.Sin(tt33); g1 = (2 * aa * bb * st3); k = (Math.Pow(cx3, 2) + Math.Pow(cy3, 2) + Math.Pow(cz3, 2) - Math.Pow(aa, 2) - Math.Pow(bb, 2)) / g1; tt23 = Math.Acos(k); k1 = bb * Math.Sin(tt33) * Math.Cos(tt23); k2 = bb * Math.Sin(tt33) * Math.Sin(tt23); tt13 = Math.Asin((k2 * cx3 - (aa + k1) * cz3) / (-Math.Pow(k2, 2) - Math.Pow(aa + k1, 2))); t13 = (tt13 * 180) / Math.PI;

En esta parte del código se realizan las operaciones necesarias para poder

obtener la velocidad de nuestro robot en cada uno de los brazos para que se

realice el control de la velocidad lo más adecuadamente posible.

// VELOCIDAD MOTOR // JX JY JZ MOTOR jx1 = Math.Cos(tt1 + tt2) * Math.Sin(tt3) * Math.Cos(o1) - Math.Cos(tt3) * Math.Sin(o1); jy1 = Math.Cos(tt1 + tt2) * Math.Sin(tt3) * Math.Sin(o1) + Math.Cos(tt3) * Math.Cos(o1); jz1 = Math.Sin(tt1 + tt2) * Math.Sin(tt3); jx2 = Math.Cos(tt12 + tt22) * Math.Sin(tt32) * Math.Cos(o2) - Math.Cos(tt32) * Math.Sin(o2); jy2 = Math.Cos(tt12 + tt22) * Math.Sin(tt32) * Math.Sin(o2) + Math.Cos(tt32) * Math.Cos(o2); jz2 = Math.Sin(tt12 + tt22) * Math.Sin(tt32); jx3 = Math.Cos(tt13 + tt23) * Math.Sin(tt33) * Math.Cos(o3) - Math.Cos(tt33) * Math.Sin(o3); jy3 = Math.Cos(tt13 + tt23) * Math.Sin(tt33) * Math.Sin(o3) + Math.Cos(tt33) * Math.Cos(o3); jz3 = Math.Sin(tt13 + tt23) * Math.Sin(tt33); // JQ MOTOR jqq1 = aa / (Math.Sin(tt2) * Math.Sin(tt3)); jqq2 = aa / (Math.Sin(tt22) * Math.Sin(tt32)); jqq3 = aa / (Math.Sin(tt23) * Math.Sin(tt33)); jq1 = (jx1 + jy1 + jz1) * jqq1; jq2 = (jx2 + jy2 + jz2) * jqq2; jq3 = (jx3 + jy3 + jz3) * jqq3; // VP MOTOR vp1 = vv1 * Math.Cos(o1) + vv2 * Math.Sin(o1); vp2 = -vv1 * Math.Sin(o1) + vv2 * Math.Cos(o1); vp3 = vv3;



// D0 MOTOR teta1 = (jq1 * vp1) + (jq1 * vp2) + (jq1 * vp3); teta2 = jq2 * vp1 + jq2 * vp2 + jq2 * vp3;

teta3 = jq3 * vp1 + jq3 * vp2 + jq3 * vp3;

Imprimimos el resultado en una caja de texto.

txtr.Clear(); txtr.Text = "Seleccionado Cinemática Inversa \r\n011=" + "" + tt1 + "\r\n" + "012= " + tt2 + "\r\n" + "013= " + tt3; tbo1.Text = " " + teta1; tbo2.Text = "" + jq1 + "" + jq2 + "" + jq3; tbo3.Text = "" + jx3 + "" + jy3 + "" + jz3; } }

La siguiente función nos sirve para poder abrir el puerto de la computadora

deseado.


{

//Si está abierto

if (serialPort1.IsOpen == true)

{

//Cerrar puerto

serialPort1.Close();

//Actualizar botón

btnabrir.Text = "Abrir Puerto";

//Actualizar etiqueta

label12.Text = "Puerto COM " + cbpuertos.SelectedItem.ToString() + "cerrado"; }



Como se puede observar en la figura 7, el nombre del botón cambia de acuerdo a

las condiciones antes descritas, si se ha abierto el puerto cambia a cerrar y si se

ha cerrado pasa lo contrario.

Es importante señalar que se debe haber seleccionado un puerto para no

ocasionar un conflicto en el software.

Figura 7.

else//Si está cerrado

{

//Primero se configura las propiedades del puerto

serialPort1.PortName = cbpuertos.SelectedItem.ToString();

//Ahora se pude abrir el puerto

serialPort1.Open();

//Actualizar “btnabrir” a “Cerrar Puerto”

btnabrir.Text = "Cerrar Puerto";

}

}

La siguiente función se utiliza para poder recibir los datos del Puerto serial, es

muy indispensable para el control de los motores pues podemos saber a través de

la recepción de los datos.

private void serialPort1_DataReceived(object sender,

SerialDataReceivedEventArgs e)



{

//Da tiempo para que lleguen todos los datos al puerto

System.Threading.Thread.Sleep(100);

//Leer datos

String datos = serialPort1.ReadExisting();

//Cambiar la propiedad Text de txtentrada de manera segura

tbre.Invoke(new EventHandler(delegate

{

tbre.AppendText(datos + "");

}));

}

Función que nos permite enviar lo que se haya escrito a través del puerto serie;

primero interpreta todo lo que este escrito, en este caso en una caja de texto y

posteriormente lo envía.


{

serialPort1.WriteLine(txtr.Text);

}

Formulario de un botón exclusive para limpiar lo que se haya enviado o recibido

en las diferentes cajas de texto.

private void button4_Click_1(object sender, EventArgs e)



{

//Caja de texto que recibe datos

tbre.Clear();

//Caja de texto que envia datos

txtr.Clear();

}

Este es el botón de simulación en el cual podemos observar cómo se mueven los

brazos de manera virtual se ilustra en la figura 8 la representación de los brazos y

su posición final utilizando la cinemática inversa.


{

//Se declara un objeto con características graficas llamado formGraphics.

Graphics formGraphics = this.CreateGraphics();

//Función que nos permite borrar lo que se haya dibujado en formGraphics.

formGraphics.Clear(Color.White);

//Se llaman a las funciones que nos van a similar el movimiento de cada uno de

los brazos.

Brazo1(); Brazo2(); Brazo3(); }



Figura 8.

Función para el movimiento simulado del Brazo 1, en esta función se declaran

algunas variables necesarias para poder realizar el movimiento vitual de cada

línea que conforma el brazo.

Nota: El resultado de las funciones trigonométricas están dadas en radianes y se

requiere usar grados, solo se necesita hacer el cálculo para convertir a grados.

private void Brazo1()

{

//Declaración de las variables

double b; double c; double hip; double tet1; double tx; double ty; double b2; double c2; double hip2 = 60; double tet2; double tx2; double ty2; double angulo2; int x; int y; int x2; int y2; int endx; int endy;

//Conversión de radianes a grados

angulo2 = 57.3 * tt2;

//Variables igualadas a datos o formular para obtener un resultado.

b = 330 - 300; c = 400 - 380; hip = Math.Sqrt((Math.Pow(c,2))+(Math.Pow(b,2))); tet1 = Math.Acos(c / hip); tet1 = tt1; tet1 = Math.Cos(tet1); tx = tet1 * hip; ty = Math.Sqrt(Math.Pow(hip,2)-Math.Pow(tx,2));



tx = Math.Round(tx); ty = Math.Round(ty); x = (int)tx; y = (int)ty;

//Tiempo que se establece para que se refresque la ventana y pinte una nueva

linea

int time = 1000;

//Se crea un objeto llamado myPen de color rojo.

myPen = new System.Drawing.Pen(System.Drawing.Color.Red); Graphics formGraphics = this.CreateGraphics();

//Para saber si la diferencia en x la necesito sumar o restar se ha creado una

sentencia en donde se pregunta si se es menor a los 90° entonces es una

diferencia que va tener que ser restada y si es mayor a 90° se le sumará.

if (t11 < 90) { tet2 = tt2; tet2 = Math.Cos(tet2); tx2 = tet2 * hip2; ty2 = Math.Sqrt(Math.Pow(hip2, 2) - Math.Pow(tx2, 2)); tx2 = Math.Round(tx2); ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2;

//Puntos declarados para el pintado de la linea en donde se le suma o resta la

diferencia en x y se le suma la diferencia en y.

endx = 400 - x; endy = 300 + y;

//Se crea una sentencia similar para el brazo de mayor longitude solo que a 45°

if (angulo2 < 45)

{

//Se dibuja una linea con un punto inicial en (400,300) y su punto final en (430,300)

formGraphics.DrawLine(myPen, 400, 300, 430, 300);



//Se hace lo mismo para poder crear las demás líneas que conformarán el brazo

pero estas estarán en función de otras variables para que puedan ser dinámicas y

se puedan mover de acuerdo a los cálculos que se obtengan anteriormente y dar

una semejanza lo más real posible del movimiento en el mundo real de este tipo

de robots.

formGraphics.DrawLine(myPen, 400, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx-x2, endy+y2); formGraphics.DrawLine(myPen, endx - x2, endy + y2, endx - x2 + 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 400, 300, 430, 300); formGraphics.DrawLine(myPen, 400, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx + x2, endy + y2); formGraphics.DrawLine(myPen, endx + x2, endy + y2, endx + x2 + 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 400, 300, 430, 300); formGraphics.DrawLine(myPen, 400, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx, endy + y2); formGraphics.DrawLine(myPen, endx, endy + y2, endx + 15, endy + y2); } } else if (t11 > 90) { tet2 = tt2; tet2 = Math.Cos(tet2); tx2 = tet2 * hip2; ty2 = Math.Sqrt(Math.Pow(hip2, 2) - Math.Pow(tx2, 2)); tx2 = Math.Round(tx2); ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2; endx = 400 + x; endy = 300 + y; if (angulo2 < 45) { formGraphics.DrawLine(myPen, 400, 300, 430, 300); formGraphics.DrawLine(myPen, 400, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx - x2, endy + y2); formGraphics.DrawLine(myPen, endx - x2, endy + y2, endx - x2 + 15, endy + y2);



} else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 400, 300, 430, 300); formGraphics.DrawLine(myPen, 400, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx + x2, endy + y2); formGraphics.DrawLine(myPen, endx + x2, endy + y2, endx + x2+15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 400, 300, 430, 300); formGraphics.DrawLine(myPen, 400, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx, endy + y2); formGraphics.DrawLine(myPen, endx, endy + y2, endx+15, endy + y2); } } else if (t11 == 90) { tet2 = tt2; tet2 = Math.Cos(tet2); tx2 = tet2 * hip2; ty2 = Math.Sqrt(Math.Pow(hip2, 2) - Math.Pow(tx2, 2)); tx2 = Math.Round(tx2); ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2; endx = 400; endy = 300 + y; if (angulo2 < 45) { formGraphics.DrawLine(myPen, 400, 300, 430, 300); formGraphics.DrawLine(myPen, 400, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx - x2, endy + y2); formGraphics.DrawLine(myPen, endx - x2, endy + y2, endx - x2+15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 400, 300, 430, 300); formGraphics.DrawLine(myPen, 400, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx + x2, endy + y2); formGraphics.DrawLine(myPen, endx + x2, endy + y2, endx + x2+15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 400, 300, 430, 300); formGraphics.DrawLine(myPen, 400, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx, endy + y2); formGraphics.DrawLine(myPen, endx, endy + y2, endx, endy + y2); }

}



//Se imprimen algunos resultados importantes de distintas variables

tbre.Text = "X= " + angulo2 + "\r\nY= " + y + "\r\nHip= " + hip + "\r\nTx= " + tx; myPen.Dispose(); formGraphics.Dispose(); }

//Función de movimiento simulado en el Brazo2 en el cual se sigue el mimso

patrón que el Brazo1

private void Brazo2() { double b; double c; double hip; double tet1; double tx; double ty; double b2; double c2; double hip2 = 60; double tet2; double tx2; double ty2; double angulo2; int x; int y; int x2; int y2; int endx; int endy; angulo2 = 57.3 * tt22; b = 330 - 300; c = 500 - 480; hip = Math.Sqrt((Math.Pow(c, 2)) + (Math.Pow(b, 2))); tet1 = Math.Acos(c / hip); tet1 = t12; tet1 = Math.Cos(tet1); tx = tet1 * hip; ty = Math.Sqrt(Math.Pow(hip, 2) - Math.Pow(tx, 2)); tx = Math.Round(tx); ty = Math.Round(ty); x = (int)tx; y = (int)ty; int time = 1000; myPen = new System.Drawing.Pen(System.Drawing.Color.Red); Graphics formGraphics = this.CreateGraphics();



if (t12 < 90) { tet2 = tt22; tet2 = Math.Cos(tet2); tx2 = tet2 * hip2; ty2 = Math.Sqrt(Math.Pow(hip2, 2) - Math.Pow(tx2, 2)); tx2 = Math.Round(tx2); ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2; endx = 500 - x; endy = 300 + y; if (angulo2 < 45) { formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx - x2, endy + y2); formGraphics.DrawLine(myPen, endx - x2, endy + y2, endx - x2 + 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx + x2, endy + y2); formGraphics.DrawLine(myPen, endx + x2, endy + y2, endx + x2 + 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx, endy + y2); formGraphics.DrawLine(myPen, endx, endy + y2, endx + 15, endy + y2); } } else if (t12 > 90) { tet2 = tt22; tet2 = Math.Cos(tet2); tx2 = tet2 * hip2; ty2 = Math.Sqrt(Math.Pow(hip2, 2) - Math.Pow(tx2, 2)); tx2 = Math.Round(tx2); ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2; endx = 500 + x; endy = 300 + y; if (angulo2 < 45)



{ formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx - x2, endy + y2); formGraphics.DrawLine(myPen, endx - x2, endy + y2, endx - x2 + 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx + x2, endy + y2); formGraphics.DrawLine(myPen, endx + x2, endy + y2, endx + x2 + 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx, endy + y2); formGraphics.DrawLine(myPen, endx, endy + y2, endx + 15, endy + y2); } } else if (t12 == 90) { tet2 = tt22; tet2 = Math.Cos(tet2); tx2 = tet2 * hip2; ty2 = Math.Sqrt(Math.Pow(hip2, 2) - Math.Pow(tx2, 2)); tx2 = Math.Round(tx2); ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2; endx = 500; endy = 300 + y; if (angulo2 < 45) { formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx - x2, endy + y2); formGraphics.DrawLine(myPen, endx - x2, endy + y2, endx - x2 + 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx + x2, endy + y2); formGraphics.DrawLine(myPen, endx + x2, endy + y2, endx + x2 + 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 500, 300, 530, 300);



formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx, endy + y2); formGraphics.DrawLine(myPen, endx, endy + y2, endx, endy + y2); } } tbre.Text = "X= " + angulo2 + "\r\nY= " + y + "\r\nHip= " + hip + "\r\nTx= " + tx; myPen.Dispose(); formGraphics.Dispose();

}

//Función de movimiento simulado en el Brazo3 en el cual se sigue el mimso

patrón que el Brazo1

private void Brazo3() { double b; double c; double hip; double tet1; double tx; double ty; double b2; double c2; double hip2 = 60; double tet2; double tx2; double ty2; double angulo2; int x; int y; int x2; int y2; int endx; int endy; angulo2 = 57.3 * tt23; b = 330 - 300; c =600 - 580; hip = Math.Sqrt((Math.Pow(c, 2)) + (Math.Pow(b, 2))); tet1 = Math.Acos(c / hip); tet1 = t13; tet1 = Math.Cos(tet1); tx = tet1 * hip; ty = Math.Sqrt(Math.Pow(hip, 2) - Math.Pow(tx, 2)); tx = Math.Round(tx); ty = Math.Round(ty); x = (int)tx; y = (int)ty;



int time = 1000; myPen = new System.Drawing.Pen(System.Drawing.Color.Red); Graphics formGraphics = this.CreateGraphics(); if (t13 < 90) { tet2 = tt23; tet2 = Math.Cos(tet2); tx2 = tet2 * hip2; ty2 = Math.Sqrt(Math.Pow(hip2, 2) - Math.Pow(tx2, 2)); tx2 = Math.Round(tx2); ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2; endx = 600 - x; endy = 300 + y; if (angulo2 < 45) { formGraphics.DrawLine(myPen, 600, 300, 430, 300); formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx - x2, endy + y2); formGraphics.DrawLine(myPen, endx - x2, endy + y2, endx - x2 + 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 600, 300, 630, 300); formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx + x2, endy + y2); formGraphics.DrawLine(myPen, endx + x2, endy + y2, endx + x2 + 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 600, 300, 630, 300); formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx, endy + y2); formGraphics.DrawLine(myPen, endx, endy + y2, endx + 15, endy + y2); } } else if (t13 > 90) { tet2 = tt23; tet2 = Math.Cos(tet2); tx2 = tet2 * hip2; ty2 = Math.Sqrt(Math.Pow(hip2, 2) - Math.Pow(tx2, 2)); tx2 = Math.Round(tx2);



ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2; endx = 600 + x; endy = 300 + y; if (angulo2 < 45) { formGraphics.DrawLine(myPen, 600, 300, 630, 300); formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx - x2, endy + y2); formGraphics.DrawLine(myPen, endx - x2, endy + y2, endx - x2 + 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 600, 300, 630, 300); formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx + x2, endy + y2); formGraphics.DrawLine(myPen, endx + x2, endy + y2, endx + x2 + 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 600, 300, 630, 300); formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx, endy + y2); formGraphics.DrawLine(myPen, endx, endy + y2, endx + 15, endy + y2); } } else if (t13 == 90) { tet2 = tt23; tet2 = Math.Cos(tet2); tx2 = tet2 * hip2; ty2 = Math.Sqrt(Math.Pow(hip2, 2) - Math.Pow(tx2, 2)); tx2 = Math.Round(tx2); ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2; endx = 600; endy = 300 + y; if (angulo2 < 45) { formGraphics.DrawLine(myPen, 600, 300, 630, 300); formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx - x2, endy + y2); formGraphics.DrawLine(myPen, endx - x2, endy + y2, endx - x2 + 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 600, 300, 630, 300);



formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx + x2, endy + y2); formGraphics.DrawLine(myPen, endx + x2, endy + y2, endx + x2 + 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 600, 300, 630, 300); formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx, endy + y2); formGraphics.DrawLine(myPen, endx, endy + y2, endx, endy + y2); } } tbre.Text = "X= " + angulo2 + "\r\nY= " + y + "\r\nHip= " + hip + "\r\nTx= " + tx; myPen.Dispose(); formGraphics.Dispose(); } } }

VENTANA DE CONFIGURACIÓN:

Esta venta es solamente para poder configurar algunos parámetros como se ha

mencionado al inicio de este documento.

//Declaramos las librerías a utilizar

using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.Linq; using System.Text; using System.Windows.Forms;

//Espacio de trabajo namespace Robot_Delta {

//Mi clase principal public partial class Configuracion : Form {

//Declaración de variables



private String h; private String r; private String o; private String b; private String ha; private String cuentas;

//Función que permite inicializar esta ventana como se muestra en la figura 9. public Configuracion() { InitializeComponent(); }

Figura 9.

//Función que permite leer el valor que se ingrese a través de una caja de texto de

manera dinámica y que estos valores se puedan utilizar posteriormente en la

ventana principal.

public String H { get { return h; } set { h = value; } }



public String R { get { return r; } set { r = value; } } public String O { get { return o; } set { o = value; } } public String B { get { return b; } set { b = value; } } public String HA { get { return ha; } set { ha = value; } } public String Cuentas { get { return cuentas; } set { cuentas = value; } }

//Formulario del botón aceptar donde se igualan las variables leídas a través de la

caja de texto.

private void btnaceptar_Click(object sender, EventArgs e) { h = tbh.Text; r = tbr.Text; b = tbb.Text; ha = tba.Text; cuentas = tbc.Text; }

//Formulario del botón Parametros predeterminados, que pone en la caja de texto

los parámetros de un robot en específico para evitar la tarea de estar ingresando

cada uno de ellos. Observar figura 10.



private void button1_Click(object sender, EventArgs e) { tbh.AppendText("3.5"); tbr.AppendText("6"); tbb.AppendText("60"); tba.AppendText("23"); }

Figura 10.

} }

manual de operaciones robot delta

Documents