You are on page 1of 49

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

Universidad Autónoma de Querétaro Facultad de Ingeniería DISEÑO E IMPLEMETACIÓN DE HARDWARE. Antes de comenzar, es
Universidad Autónoma de Querétaro Facultad de Ingeniería DISEÑO E IMPLEMETACIÓN DE HARDWARE. Antes de comenzar, es

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.

BRAZO DELTA Encoder Halls Alimentación
BRAZO DELTA
Encoder
Halls
Alimentación
SERVO CONTROL
SERVO CONTROL
PC Comunicación RS232
PC
Comunicación
RS232
Universidad Autónoma de Querétaro Facultad de Ingeniería DISEÑO E IMPLEMETACIÓN DE HARDWARE. Antes de comenzar, es
Universidad Autónoma de Querétaro Facultad de Ingeniería DISEÑO E IMPLEMETACIÓN DE HARDWARE. Antes de comenzar, es
DISEÑO MECÁNICO
DISEÑO MECÁNICO
DISEÑO ELECTRÓNICO
DISEÑO ELECTRÓNICO

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.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 2. Arquitectura y distribución geométrica de un brazo
Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 2. Arquitectura y distribución geométrica de un brazo
Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 2. Arquitectura y distribución geométrica de un brazo

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

links

(9mm).

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.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Para alimentar la etapa de control, se utiliza una
Universidad Autónoma de Querétaro Facultad de Ingeniería Para alimentar la etapa de control, se utiliza una

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.

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Para alimentar la etapa de control, se utiliza una

Fuente 24VDC

10 Amperes.

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.

Universidad Autónoma de Querétaro Facultad de Ingeniería Para alimentar la etapa de control, se utiliza una

Motores.

Figura 4. Circuito de distribución de carga.

Los motores utilizados para esta aplicación son los que se muestran en la foto. Motores sin escobillas de DC.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Y le corresponden estas características: NOTA: los datos fueron
Universidad Autónoma de Querétaro Facultad de Ingeniería Y le corresponden estas características: NOTA: los datos fueron
Universidad Autónoma de Querétaro Facultad de Ingeniería Y le corresponden estas características: NOTA: los datos fueron

Y le corresponden estas características:

Universidad Autónoma de Querétaro Facultad de Ingeniería Y le corresponden estas características: NOTA: los datos fueron

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.
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:

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autónoma de Querétaro Facultad de Ingeniería Hacia el servocontrol Hacia los motores Como se puede

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

Hacia el

servocontrol
servocontrol
Universidad Autónoma de Querétaro Facultad de Ingeniería Hacia el servocontrol Hacia los motores Como se puede

Hacia los

motores

Universidad Autónoma de Querétaro Facultad de Ingeniería Hacia el servocontrol Hacia los motores Como se puede

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

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería de los 3 se le ha hecho la petición
Universidad Autónoma de Querétaro Facultad de Ingeniería de los 3 se le ha hecho la petición

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.

M3 M2 M1
M3
M2
M1

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

LED PIN_C0
LED
PIN_C0

LED indicador

de alimentación

QEI
QEI
RESET PIC ICSP Alimentación
RESET
PIC
ICSP
Alimentación

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.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Etapa de servo amplificación. Esta contiene todos los servodrivers
Universidad Autónoma de Querétaro Facultad de Ingeniería Etapa de servo amplificación. Esta contiene todos los servodrivers

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:

Amplificador DAC REFERENCIA Alimentación
Amplificador
DAC
REFERENCIA
Alimentación

servodriver

Universidad Autónoma de Querétaro Facultad de Ingeniería Etapa de servo amplificación. Esta contiene todos los servodrivers

Comunicación Serial.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Para la comunicación serial fue requerido un circuito lógico
Universidad Autónoma de Querétaro Facultad de Ingeniería Para la comunicación serial fue requerido un circuito lógico

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:

RX TX /M1 RX TX /M3 RX TX /M2 RX TX /MAX RX TX GND /PC
RX
TX /M1
RX
TX /M3
RX
TX /M2
RX
TX /MAX
RX
TX GND /PC

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.

Universidad Autónoma de Querétaro Facultad de Ingeniería Para la comunicación serial fue requerido un circuito lógico

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería PROGRAMA DE LOS CONTROLADORES. PID DE LOS MICROS El
Universidad Autónoma de Querétaro Facultad de Ingeniería PROGRAMA DE LOS CONTROLADORES. PID DE LOS MICROS El

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.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería //################## VARIABLES PID ############################################### int16 cuentas = 0; int16
Universidad Autónoma de Querétaro Facultad de Ingeniería //################## VARIABLES PID ############################################### int16 cuentas = 0; int16

//################## VARIABLES PID ###############################################

int16 cuentas = 0; int16 control = 0;

//lectura de cuentas //valor

int16 posRef = 0;

//referencia de posicion

float rT, eT, yT, uT, iT_1, eT_1; float max, min; float Kp, Ki, Kd; float senalControl = 0; float pT = 0, qT = 0; float pT_1 = 0.0;

//variables de ecuaciones //límites máximo y mínimo de control. //constantes del PID

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++) buf[y] = 0;

// Coloca los valores del buffer en 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.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería #INT_TIMER2 // Interrupción del TIMER 2 void intTimer(){ if(flagPID){
Universidad Autónoma de Querétaro Facultad de Ingeniería #INT_TIMER2 // Interrupción del TIMER 2 void intTimer(){ if(flagPID){

#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; rT = (float)posRef;

// Se muestrea el numero de cuentas

eT = rT - yT;

//Cálculo error

pT = (0.001*eT) + pT_1; qT = Kd*((eT-eT_1)/0.001); uT = (Ki*pT) + (Kp*eT) + qT;

//Cálculo del término integral //Cálculo del término derivativo //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; output_b (control);

//Transferencia de salida PID

pT_1 = pT; eT_1 = eT;

//Guardar variables

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

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

void main()

{

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

//--------------------- Define states on State Machine STM -----------------

void main() { Universidad Autónoma de Querétaro Facultad de Ingeniería //--------------------- Define states on State Machine
void main() { Universidad Autónoma de Querétaro Facultad de Ingeniería //--------------------- Define states on State Machine

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.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería 2. Al tener buenos resultados con el primer paso
Universidad Autónoma de Querétaro Facultad de Ingeniería 2. Al tener buenos resultados con el primer paso
  • 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;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería //############################# STATE 0 ####################################### case STMST0: if(str_flag) { if
Universidad Autónoma de Querétaro Facultad de Ingeniería //############################# STATE 0 ####################################### case STMST0: if(str_flag) { if

//############################# 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

output_high(PIN_C5);

printf("C%d = %ld

\r\n",id, cuentas);

output_low(PIN_C5);

currentState= STMST0;

movido // el motor. obsoluto.

Este

valor

es

un

valor

// Manda por el puerto UART a la PC la posición, en cuentas, de la flecha del motor.

break;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería //############################# STATE 2 ####################################### case STMST2: //RESET. qei_set_count(0); output_high(PIN_C5);
Universidad Autónoma de Querétaro Facultad de Ingeniería //############################# STATE 2 ####################################### case STMST2: //RESET. qei_set_count(0); output_high(PIN_C5);

//############################# STATE 2 #######################################

case STMST2: //RESET.

qei_set_count(0);

output_high(PIN_C5);

printf(" M%d Listo \n\r", id);

output_low(PIN_C5);

currentState=STMST0;

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

// 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.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Es recomendable para el mejor entendimiento del programa tener
Universidad Autónoma de Querétaro Facultad de Ingeniería Es recomendable para el mejor entendimiento del programa tener

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.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería INTERFAZ GRÁFICA PARA EL CONTROL DEL ROBOT DELTA Se
Universidad Autónoma de Querétaro Facultad de Ingeniería INTERFAZ GRÁFICA PARA EL CONTROL DEL ROBOT DELTA Se

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:

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería A i B i  B i C i
Universidad Autónoma de Querétaro Facultad de Ingeniería A i B i  B i C i

AiBi BiCi OP PCi OAi

Los cuales contienen los siguientes datos:

acos1i

     

bsin3i cos1i 2i

     

cosi sini 0

   

Px

     

h r

0

bcos3i

sini cosi 0

Py

0

asin1i

bsin3i sin1i 2i

0

0

1

Pz

0

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

“end efector” se encuentra el siguiente resultado:

Cx Px cosiPysinih r Cy Px siniPycosi

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

sin 1 k2Cxak1Cz k 2 2 ak12

2i

cos 1 Cx 2 Cy 2ab 2 Cz sin3i2 a 2 b 2

3i cos 1 Px siniPycosi b

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Plataforma de programación:

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

Plataforma de programación: Universidad Autónoma de Querétaro Facultad de Ingeniería Para poder desarrollar esta interfaz fue
Plataforma de programación: Universidad Autónoma de Querétaro Facultad de Ingeniería Para poder desarrollar esta interfaz fue

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.

Plataforma de programación: Universidad Autónoma de Querétaro Facultad de Ingeniería Para poder desarrollar esta interfaz fue

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.

Plataforma de programación: Universidad Autónoma de Querétaro Facultad de Ingeniería Para poder desarrollar esta interfaz fue

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.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Explicación del programa desarrollado:

VENTANA PRINCIPAL FORM1:

Universidad Autónoma de Querétaro Facultad de Ingeniería Explicación del programa desarrollado: VENTANA PRINCIPAL FORM1: Las siguientes
Universidad Autónoma de Querétaro Facultad de Ingeniería Explicación del programa desarrollado: VENTANA PRINCIPAL FORM1: Las siguientes

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;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

//Libreria para hacer dibujos en 2D

using System.Drawing.Drawing2D;

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

//Libreria para hacer dibujos en 2D using System.Drawing.Drawing2D; Universidad Autónoma de Querétaro Facultad de Ingeniería El
//Libreria para hacer dibujos en 2D using System.Drawing.Drawing2D; Universidad Autónoma de Querétaro Facultad de Ingeniería El

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;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

private String b; private String ha; private String v1; private String v2; private String v3; private String cuenta;

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

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;

private String b; private String ha; private String v1; private String v2; private String v3; private
private String b; private String ha; private String v1; private String v2; private String v3; private

Con la siguiente function se inicializa nuestra interfaz. Observar figura 1.

public Form1()

{

 

InitializeComponent();

}

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 1. El programa cuenta con dos funciones adicionales
Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 1. El programa cuenta con dos funciones adicionales
Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 1. El programa cuenta con dos funciones adicionales

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;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

}

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

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

Figura 2.

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

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;

}

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 3. La siguiente función viene dada por el
Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 3. La siguiente función viene dada por el
Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 3. La siguiente función viene dada por el

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) {

Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 3. La siguiente función viene dada por el

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();

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 5. Una vez que se de click en

Figura 5.

Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 5. Una vez que se de click en
Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 5. Una vez que se de click en

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;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

}

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

} Universidad Autónoma de Querétaro Facultad de Ingeniería Una vez que se abre la ventana de
} Universidad Autónoma de Querétaro Facultad de Ingeniería Una vez que se abre la ventana de

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);

}

}

} Universidad Autónoma de Querétaro Facultad de Ingeniería Una vez que se abre la ventana de

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.

} Universidad Autónoma de Querétaro Facultad de Ingeniería Una vez que se abre la ventana de

private void button2_Click(object sender, EventArgs e) {

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Dependiendo del tipo de cálculo que se requiera en
Universidad Autónoma de Querétaro Facultad de Ingeniería Dependiendo del tipo de cálculo que se requiera en

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 Cy   Px Px cosisini  PysiniPycosih r

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;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

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;

Universidad Autónoma de Querétaro Facultad de Ingeniería v3 = tbv3.Text; ppx = double .Parse(px); ppy =
Universidad Autónoma de Querétaro Facultad de Ingeniería v3 = tbv3.Text; ppx = double .Parse(px); ppy =

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 cos 1 Px siniPycosi b

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

Se determina el ángulo ϴ 2:

2i cos 1 Cx 2 Cy 2ab 2 Cz sin3i2 a 2 b 2

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:

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

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

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

Universidad Autónoma de Querétaro Facultad de Ingeniería k1=bb* Math .Sin(tt3)* Math .Cos(tt2); k2=bb* Math .Sin(tt3)* Math
Universidad Autónoma de Querétaro Facultad de Ingeniería k1=bb* Math .Sin(tt3)* Math .Cos(tt2); k2=bb* Math .Sin(tt3)* Math

Se determina el ángulo ϴ 1:

1i sin 1 k2Cxak1Cz k 2 2 ak12

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));

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

st3 = Math.Sin(tt33);

Universidad Autónoma de Querétaro Facultad de Ingeniería st3 = Math .Sin(tt33); g1 = (2 * aa
Universidad Autónoma de Querétaro Facultad de Ingeniería st3 = Math .Sin(tt33); g1 = (2 * aa

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;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

// D0 MOTOR

teta1 = (jq1 * vp1) + (jq1 * vp2) + (jq1 * vp3);

teta2 = jq2 * vp1 + jq2 * vp2 + jq2 * vp3;

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

Universidad Autónoma de Querétaro Facultad de Ingeniería // D0 MOTOR teta1 = (jq1 * vp1) +
Universidad Autónoma de Querétaro Facultad de Ingeniería // D0 MOTOR teta1 = (jq1 * vp1) +

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.

Universidad Autónoma de Querétaro Facultad de Ingeniería // D0 MOTOR teta1 = (jq1 * vp1) +

private void button4_Click(object sender, EventArgs e) {

//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";

}

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería Como se puede observar en la figura 7, el
Universidad Autónoma de Querétaro Facultad de Ingeniería Como se puede observar en la figura 7, el

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.

Universidad Autónoma de Querétaro Facultad de Ingeniería Como se puede observar en la figura 7, el

else//Si está cerrado {

Figura 7.

//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)

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

{

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

{ Universidad Autónoma de Querétaro Facultad de Ingeniería //Da tiempo para que lleguen todos los datos
{ Universidad Autónoma de Querétaro Facultad de Ingeniería //Da tiempo para que lleguen todos los datos

//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.

{ Universidad Autónoma de Querétaro Facultad de Ingeniería //Da tiempo para que lleguen todos los datos

private void button3_Click(object sender, EventArgs e) {

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.

{ Universidad Autónoma de Querétaro Facultad de Ingeniería //Da tiempo para que lleguen todos los datos

private void button4_Click_1(object sender, EventArgs e)

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

{

//Caja de texto que recibe datos

tbre.Clear();

//Caja de texto que envia datos

txtr.Clear();

}

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

{ //Caja de texto que recibe datos tbre.Clear(); //Caja de texto que envia datos txtr.Clear(); }
{ //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.

private void button5_Click(object sender, EventArgs e) {

//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();

}

{ //Caja de texto que recibe datos tbre.Clear(); //Caja de texto que envia datos txtr.Clear(); }

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Figura 8.

Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 8. Función para el movimiento simulado del Brazo
Universidad Autónoma de Querétaro Facultad de Ingeniería Figura 8. Función para el movimiento simulado del Brazo

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));

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

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

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

//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);

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería //Se hace lo mismo para poder crear las demás
Universidad Autónoma de Querétaro Facultad de Ingeniería //Se hace lo mismo para poder crear las demás

//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);

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

} else if (angulo2 > 45) {

Universidad Autónoma de Querétaro Facultad de Ingeniería } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 400,
Universidad Autónoma de Querétaro Facultad de Ingeniería } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 400,

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);

}

}

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería //Se imprimen algunos resultados importantes de distintas variables tbre.Text
Universidad Autónoma de Querétaro Facultad de Ingeniería //Se imprimen algunos resultados importantes de distintas variables tbre.Text

//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();

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

if (t12 < 90) {

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

if (t12 < 90) { Universidad Autónoma de Querétaro Facultad de Ingeniería tet2 = tt22; tet2
if (t12 < 90) { Universidad Autónoma de Querétaro Facultad de Ingeniería tet2 = tt22; tet2

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)

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

{

Universidad Autónoma de Querétaro Facultad de Ingeniería { formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300,
Universidad Autónoma de Querétaro Facultad de Ingeniería { formGraphics.DrawLine(myPen, 500, 300, 530, 300); formGraphics.DrawLine(myPen, 500, 300,

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);

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx,
Universidad Autónoma de Querétaro Facultad de Ingeniería formGraphics.DrawLine(myPen, 500, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx,

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;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería int time = 1000; myPen = new System.Drawing. Pen
Universidad Autónoma de Querétaro Facultad de Ingeniería int time = 1000; myPen = new System.Drawing. Pen

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);

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2;

endx = 600 + x; endy = 300 + y;

if (angulo2 < 45) {

Universidad Autónoma de Querétaro Facultad de Ingeniería ty2 = Math .Round(ty2); x2 = ( int )tx2;
Universidad Autónoma de Querétaro Facultad de Ingeniería ty2 = Math .Round(ty2); x2 = ( int )tx2;

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);

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

Universidad Autónoma de Querétaro Facultad de Ingeniería formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx
Universidad Autónoma de Querétaro Facultad de Ingeniería formGraphics.DrawLine(myPen, 600, 300, endx, endy); formGraphics.DrawLine(myPen, endx, endy, endx

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

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

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

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

//Función que permite inicializar esta ventana como se muestra en la figura 9.

public Configuracion() {

InitializeComponent();

}

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

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; }

}

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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; }

}

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

public String R { get { return r; } set { r = value ; }
public String R { get { return r; } set { r = value ; }

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

public String R { get { return r; } set { r = value ; }

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.

public String R { get { return r; } set { r = value ; }

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

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

private void button1_Click(object sender, EventArgs e) {

tbh.AppendText("3.5");

tbr.AppendText("6");

tbb.AppendText("60");

tba.AppendText("23");

}

Universidad Autónoma de Querétaro Facultad de Ingeniería private void button1_Click( object sender, EventArgs e) { tbh.AppendText(

}

}

Figura 10.

Universidad Autónoma de Querétaro Facultad de Ingeniería private void button1_Click( object sender, EventArgs e) { tbh.AppendText(
Universidad Autónoma de Querétaro Facultad de Ingeniería private void button1_Click( object sender, EventArgs e) { tbh.AppendText(

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto