Manual de Operaciones Robot Delta

Universidad Autnoma de Quertaro Facultad de Ingeniera
DISEO E IMPLEMETACIN 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
SERVO CONTROL
Comunicacin RS232
PC
Alimentacin
DISEO MECNICO
DISEO ELECTRNICO Figura 1. Arquitectura del sistema.
DISEO MECNICO. El brazo delta consta de 3 Actuadores dispersos uno del otro cada 120 grados, cada uno acta independientemente, pero es necesario recordar que un mecanismo paralelo. Es sumamente importante recordar esto en el momento de obtener los clculos. Est estructurado de brazos, vnculos l (links) y uniones j (joints) tanto esfricas 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
Figura 2. Arquitectura y distribucin geomtrica de un brazo delta.
Materiales utilizados:
Material Perfil cuadrados de Aluminio (3/4). Canales de Aluminio (9mm). Ejes de acero. Cojinetes para los ejes. Anillos de retencin para los ejes. PTR (2 in). Madera Uniones Uniones: para soportar y disminuir friccin de los ejes. Uniones: para restringir que el eje se moviera de su posicin preestablecida. Estructura. Estructura: Soporte de los Actuadores. (servomotores); Soporte para la interfaz de servocontrol. Gomas de plstico. Utilizadas como coples para la unin Motor-Brazo. links Brazos Lugar de uso.
DISEO ELECTRNICO. Alimentacin.
Arana Ruiz. Diego
Para alimentar la etapa de control, se utiliza una fuente de computadora, as como tambin para alimentar la tarjeta que contiene al DAC y el Amplificador, pues es requerida una fuente simtrica de 12VDC. Para la alimentacin 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 Fuente 24VDC 10 Amperes.
Figura 3. Fuente Simtrica de computadora y fuente de 24VDC a 10 A.
Otro circuito se utiliza como receptor de la alimentacin proveniente de las dos fuentes, para as despus ser distribuida la corriente hacia todo el sistema. Se muestra a continuacin el circuito mencionado.
Figura 4. Circuito de distribucin de carga.
Motores. Los motores utilizados para esta aplicacin son los que se muestran en la foto. Motores sin escobillas de DC.
Arana Ruiz. Diego
Y le corresponden estas caractersticas:
NOTA: los datos fueron obtenidos a partir del anlisis de seales provenientes del motor, puesto que no existe una hoja de datos o especificaciones en internet para este motor.
Para la distribucin de corriente de los motores se hizo un circuito que acta como receptor de las 3 fases y la alimentacin del encoder, y como emisor de las seales provenientes del encoder y de los sensores de efecto hall. A continuacin se muestra la imagen de cmo fueron distribuidas las seales:
Arana Ruiz. Diego

Hacia el servocontrol Hacia los motores
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 Electrnica del servocontrol se divide en 2 partes principales: 1. Etapa de control. 2. Etapa de servo amplificacin. Etapa de control. Se utiliz un PIC18f2331 por sus caractersticas especiales (consultar datasheet), cabe mencionar que no se explot el potencial del micocontrolador, pero la tarjeta est diseada 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. Ms adelante se describe el diseo del software. A la salida de este, enva por el puerto B una seal digital de 8 bits para posteriormente ser convertida a analgica en la etapa de servo-amplificacin. 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
de los 3 se le ha hecho la peticin de ejecucin. En la imagen(Figura 2) se muestra a quien corresponde cada etiqueta.
M3
M2
M1
A continuacin se detalla la tarjeta que contiene al PIC.
LED PIN_C0
LED indicador de alimentacin
QEI
RESET
PIC
ICSP Alimentacin
Nota: El PIC M3 posee una diferencia entre los dems, pues por defectos de construccin 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 comunicacin es con 3 PICs, para que no se mezclen las seales.
Arana Ruiz. Diego
Etapa de servo amplificacin. Esta contiene todos los servodrivers y la seal 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 configuracin (En caso de no tener conectado nada o no tener conectores, recurrir directamente al datasheet y crear una nueva conexin 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 seal aun debe ser amplificada, es por eso que un amplificador debe cumplir esta funcin. A continuacin se muestra el circuito utilizado:
Amplificador Alimentacin DAC REFERENCIA servodriver
Comunicacin Serial.
Arana Ruiz. Diego
Para la comunicacin serial fue requerido un circuito lgico para no cruzar la comunicacin entre dispositivos. A continuacin se muestra el circuito referido y como debe ser conectado con la tarjeta que contiene al MAX232:
RX TX /M3 RX TX /M1
RX TX /M2
RX TX /MAX
RX TX GND /PC
Puesto que no es muy clara la imagen en la parte rodeada por el crculo, a continuacin se ejemplifica las conexiones seriales para el PIC.
Arana Ruiz. Diego
Universidad Autnoma de Quertaro Facultad de Ingeniera PROGRAMA DE LOS CONTROLADORES. PID DE LOS MICROS
El programa de control para los motores se realizo mediante el uso de MICROCONTROLADORES, especficamente el PIC 18F2331 de MICROCHIP, el cual tiene con un mdulo de cuadratura que nos permite monitorear las cuentas del encoder y de esta manera realizar un lazo de control. A continuacin se colocara el cdigo fuente del controlador y se explicara lnea a lnea 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) // Lneas 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 tamao 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.
Arana Ruiz. Diego

//################## 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; float max, min; float Kp, Ki, Kd; float senalControl = 0; float pT = 0, qT = 0; float pT_1 = 0.0; int id = 0; 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] == '.') { str_flag = 1; buf[i+1] = 0; strcpy (str,buf); for(y=0; y< buf_length ;y++) buf[y] = 0; //variables de ecuaciones //lmites mximo y mnimo de control. //constantes del PID
//identificador del PIC/MOTOR
// Ingresa a esta opcin hasta que encuentra un . // Activa la bandera que indica que recibi una instruccin. // Copia el buffer a la cadena de comparacin. // Coloca los valores del buffer en 0
i = 0; } else{ i++; } } // Funcin de la interrupcin 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 carcter copia el buffer sobre la cadena de comparacin.
Arana Ruiz. Diego

#INT_TIMER2 void intTimer(){ if(flagPID){ // Interrupcin del TIMER 2
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; pT = (0.001*eT) + pT_1; qT = Kd*((eT-eT_1)/0.001); uT = (Ki*pT) + (Kp*eT) + qT; if (uT > max) { uT = max; }else if (uT < min){ uT = min; } control = (int16)uT; output_b (control); pT_1 = pT; eT_1 = eT;
//Clculo error //Clculo del trmino integral //Clculo del trmino derivativo //Clculo de la salida PID //Salida PID si es mayor que el MAX //Salida PID si es menor que el MIN
//Transferencia de salida PID
//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 interrupcin se realizan los clculos del PID, el cual manda la seal de control a la etapa de servo amplificacin.
//########################################################################## MAIN
Arana Ruiz. Diego

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 mdulo 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 // Valor mnimo de la seal de control // Valor mximo de la seal de control // Referencia de posicin a alcanzar (en cuentas)
min = 0.0; max = 255.0; posRef = 500.0;
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 reaccin del control y con esto comenzar a variar la Kp hasta observar que el motor llegue a una posicin 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 posicin a la cual haba llegado anteriormente, no importa que exista un error muy grande entre la posicin de la flecha y la referencia.
Arana Ruiz. Diego
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 ms 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 cdigo para ingresar al resto del programa, en este caso su cdigo es M1 break;
Arana Ruiz. Diego
//############################# 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 nmero 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 posicin, en cuentas, de la flecha del motor. break;
Arana Ruiz. Diego

//############################# 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; // Resetea el mdulo de cuadratura, igualando la posicin 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 posicin. break; //########################## Default ########################################## default: currentState=STMST0; // El estado de dafeault nos manda al estado 0. break; } } } // Termina el programa. // El valor del mdulo de cuadratura es igual a lo que se escriba con //esta funcin.
Arana Ruiz. Diego
Es recomendable para el mejor entendimiento del programa tener nociones del concepto de Mquina de Estados y de estudiar en la ayuda del compilador CCS las funciones que no sean entendibles para el usuario.
Arana Ruiz. Diego
INTERFAZ GRFICA PARA EL CONTROL DEL ROBOT DELTA
Se deber desarrollar un programa que cumpla las necesidades de realizar los clculos 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 clculo de 1 se necesitan calcular primero los parmetros de 2 y 3, adems 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 articulacin de menor tamao.
b = Es la longitud de la articulacin de mayor tamao.
Tambin se necesita que el programa calcule las velocidades lineales; y que todos estos datos sean enviados a travs de un puerto de la computadora, para este caso a travs del puerto serie.
Clculos
Para poder determinar cules sern los valores de nuestros ngulos y saber la posicin del end efector necesitamos realizar y resolver una serie de ecuaciones.
Primero es necesario determinar los vectores de nuestro robot como se muestra a continuacin:
Arana Ruiz. Diego
A i Bi Bi C i OP PC i OA i
Los cuales contienen los siguientes datos:
a cos 1i 0 a sin 1i
b sin 3i cos 1i 2i b cos 3i b sin 3i sin 1i 2i
cos i 0
sin i 0 0 1
Px Py Pz
hr 0 0
sin i cos i 0
Despejando de los vectores anteriores, para poder determinar la posicin final del end efector se encuentra el siguiente resultado:
Cx Px cos i Pysin i h r
Cy Px sin i Pycos i
Cz Pz
Posteriormente de determinar el resultado de la posicin podemos hacer el despeje de ecuaciones y la relacin entre ellas para encontrar las ecuaciones que rigen los ngulos en cada uno de los brazos del robot delta.
1i sin1
k 2 Cxak 1 Cz k 2 ak 1 2 2
2i cos 1
3i cos 1
Cx 2 Cy 2 Cz 2 a 2 b 2 2ab sin 3i
Px sin i Pycos i b
Arana Ruiz. Diego
Plataforma de programacin:
Para poder desarrollar esta interfaz fue necesario hacer la descargar y la instalacin de Microsoft Visual C#, el cual puede ser conseguido desde internet de forma gratuita, en la pgina oficial de Microsoft.
Al momento de la descarga se puede realizar la instalacin del mismo, una vez seleccionado la ruta de instalacin. 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 instalacin, 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 trmino de la instalacin y el registro del producto se puede empezar a programar de manera dinmica y muy grafico-visual.
Se ha desarrollado el programa en este ambiente por las facilidades de programacin que otorga, ya que nos ofrece una programacin 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.
Arana Ruiz. Diego
Explicacin del programa desarrollado:

VENTANA PRINCIPAL FORM1:
Las siguientes libreras son indispensables en el programa pues dan las herramientas necesarias para que el programa se lleve a cabo segn lo planeado.
//Es indispensable para las aplicaciones de sistema

using System;
// Librera para objetos genricos

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;
Arana Ruiz. Diego
//Libreria para hacer dibujos en 2D

using System.Drawing.Drawing2D;
El namespace es el espacio en cual se desarrolla cualquier cdigo 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 prodrmos ejecutar las funciones que sean necesarias para que el programa funcione adecuadamente
public partial class Form1 : Form {
Abajo se tiene la declaracin de cada una de las variables que necesitamos para poder realizar los clculos.
private private private private private private private private private private private private private private private private private private private String t1,t2,t3; String px, py, pz; Double tt1, tt2, tt3; Double tt12, tt22, tt32; Double tt13, tt23, tt33; Double t11, t12, t13; Double ppx, ppy, ppz; Double cx, cy, cz; Double cx2, cy2, cz2; Double cx3, cy3, cz3; Double k,k1, k2; Double jx1, jy1, jz1; Double jx2, jy2, jz2; Double jx3, jy3, jz3; Double jq1, jq2, jq3; Double jqq1, jqq2, jqq3; Double vp1, vp2, vp3; Double teta1, teta2, teta3; Pen myPen;
private String p; private String r; private String o;
Arana Ruiz. Diego

private private private private private private private private private private private private private private private private private String String String String String String b; ha; v1; v2; v3; cuenta;
float hh; float rr; float o1= 0; double o2= (120 * Math.PI)/180; double o3 = (240 * Math.PI) / 180; float bb; float aa; float cuentas; float vv1; float vv2; float vv3;
Con la siguiente function se inicializa nuestra interfaz. Observar figura 1.

public Form1() { InitializeComponent(); }
Arana Ruiz. Diego
Figura 1.
El programa cuenta con dos funciones adicionales ya que los clculos para posicionar el end efector los podemos hacer a travs de la cinemtica directa o inversa, la cual es posible seleccionarla a travs de una funcin dinmica llama radio button donde solo es necesario dar un click en cualquiera de las dos opciones para que sea posible configurar respectivos parmetros de informacin 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;
Arana Ruiz. Diego

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; }
Arana Ruiz. Diego
Figura 3. La siguiente funcin viene dada por el formulario de un botn en el cual realiza abre otra ventana para poder configurar parmetros de un robot en especifico. Observar figura 4.
private void button1_Click(object sender, EventArgs e) {
Figura 4. En las siguientes dos lneas de cdigo se define a un objeto conf que va tener las propiedades y atributos que mi funcin Configuracion en esta funcin
configuracin se puede modificar las caractersticas del robot. Observar figura 5.

Configuracion conf = new Configuracion(); DialogResult resulta = conf.ShowDialog();
Arana Ruiz. Diego
Figura 5.
Una vez que se de click en el botn aceptar todos las especificaciones que hayamos puesto en la ventana configuracin se van igualar a otras varibales con el fin de hacer los clculos 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;
Arana Ruiz. Diego

}
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 funcin. 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 clculos 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 configuracin del robot, sin estos datos posiblemente nos arroje un error que el programa lo interpreta como datos insuficientes.
Arana Ruiz. Diego
Dependiendo del tipo de clculo que se requiera en este botn se reconocer si se ha seleccionado cinemtica inversa o directa a travs de la siguiente instruccin:
if (rbdirecta.Checked == true) {
//Variables para Cinemtica Directa

t1 = tbt1.Text; t2 = tbt2.Text; t3 = tbt3.Text; tt1 = float.Parse(t1); tt2 = float.Parse(t2); tt3 = float.Parse(t3);
Los siguientes clculos son indispensables para poder saber el punto final del end efector.
Cx Px cos i Pysin i h r
Cy Px sin i Pycos i
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 Cinemtica directa" + "\r\n\r\nResultado: " + "\r\nCx= " + cx + "\r\nCy= "+cy+"\r\nCz= "+cz; } if (rbinversa.Checked == true) {
//Variables para Cinemtica Inversa

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

v3 = tbv3.Text; ppx ppy ppz vv1 vv2 vv3 = = = = = = double.Parse(px); double.Parse(py); double.Parse(pz); float.Parse(v1); float.Parse(v2); 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 cinemtica inversa es necesario encontrar los ngulos, que estn inmersos en las ecuaciones para el control de posicin de los brazos.
s1= Math.Sin(o1); c1 = Math.Cos(o1);
Se determina el ngulo 3:
3i cos 1
Px sin i Pycos i b
tt3 =Math.Acos(((-ppx * s1)+(ppy * c1))/ (bb)); st3 = Math.Sin(tt3);
2i cos 1
Cx 2 Cy 2 Cz 2 a 2 b 2 2ab sin 3i
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 clculo de 1:
Arana Ruiz. Diego

k1=bb*Math.Sin(tt3)*Math.Cos(tt2); k2=bb*Math.Sin(tt3)*Math.Sin(tt2);
1i sin1
k 2 Cxak 1 Cz k 2 ak 1 2 2
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 clculo de la posicin 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));
Arana Ruiz. Diego

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 cdigo 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 ms 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 vp1 = vp2 = vp3 = MOTOR vv1 * Math.Cos(o1) + vv2 * Math.Sin(o1); -vv1 * Math.Sin(o1) + vv2 * Math.Cos(o1); vv3;
Arana Ruiz. Diego

// 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 Cinemtica Inversa \r\n011=" + "" + tt1 + "\r\n" + "012= tbo1.Text = tbo2.Text = tbo3.Text = } } " + tt2 + "\r\n" + "013= " + tt3; " " + teta1; "" + jq1 + "" + jq2 + "" + jq3; "" + jx3 + "" + jy3 + "" + jz3;
La siguiente funcin nos sirve para poder abrir el puerto de la computadora deseado.
//Si est abierto

if (serialPort1.IsOpen == true) {
//Cerrar puerto
serialPort1.Close();
//Actualizar botn
btnabrir.Text = "Abrir Puerto";
//Actualizar etiqueta
label12.Text = "Puerto COM " + cbpuertos.SelectedItem.ToString() + "cerrado"; }
Arana Ruiz. Diego
Como se puede observar en la figura 7, el nombre del botn 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 sealar 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 funcin se utiliza para poder recibir los datos del Puerto serial, es muy indispensable para el control de los motores pues podemos saber a travs de la recepcin de los datos.
private void serialPort1_DataReceived(object sender, SerialDataReceivedEventArgs e)
Arana Ruiz. Diego

{
//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 + ""); })); }
Funcin que nos permite enviar lo que se haya escrito a travs del puerto serie; primero interpreta todo lo que este escrito, en este caso en una caja de texto y posteriormente lo enva.
private void button3_Click(object sender, EventArgs e) { serialPort1.WriteLine(txtr.Text); }
Formulario de un botn 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)
Arana Ruiz. Diego

{
//Caja de texto que recibe datos

tbre.Clear();
//Caja de texto que envia datos

txtr.Clear(); }
Este es el botn de simulacin en el cual podemos observar cmo se mueven los brazos de manera virtual se ilustra en la figura 8 la representacin de los brazos y su posicin final utilizando la cinemtica inversa.
//Se declara un objeto con caractersticas graficas llamado formGraphics.

Graphics formGraphics = this.CreateGraphics();
//Funcin 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(); }
Arana Ruiz. Diego
Figura 8.
Funcin para el movimiento simulado del Brazo 1, en esta funcin se declaran algunas variables necesarias para poder realizar el movimiento vitual de cada lnea que conforma el brazo.
Nota: El resultado de las funciones trigonomtricas estn dadas en radianes y se requiere usar grados, solo se necesita hacer el clculo para convertir a grados.
private void Brazo1() {
//Declaracin 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;
//Conversin 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));
Arana Ruiz. Diego

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);
Arana Ruiz. Diego
//Se hace lo mismo para poder crear las dems lneas que conformarn el brazo pero estas estarn en funcin de otras variables para que puedan ser dinmicas y se puedan mover de acuerdo a los clculos que se obtengan anteriormente y dar una semejanza lo ms 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, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2);
400, 300, 430, 300); 400, 300, endx, endy); endx, endy, endx - x2, endy + y2); endx - x2, endy + y2, endx - x2 +
Arana Ruiz. Diego

} else if (angulo2 > 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 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, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, }
400, 300, 430, 300); 400, 300, endx, endy); endx, endy, endx + x2, endy + y2); endx + x2, endy + y2, endx + x2+15,
400, 300, 430, 300); 400, 300, endx, endy); endx, endy, endx, endy + y2); endx, endy + y2, endx+15, endy +
400, 300, 430, 300); 400, 300, endx, endy); endx, endy, endx - x2, endy + y2); endx - x2, endy + y2, endx - x2+15,
400, 300, 430, 300); 400, 300, endx, endy); endx, endy, endx + x2, endy + y2); endx + x2, endy + y2, endx + x2+15,
400, 300, 430, 300); 400, 300, endx, endy); endx, endy, endx, endy + y2); endx, endy + y2, endx, endy + y2);
}
Aguilar Hdez. Gustavo Arana Ruiz. Diego Torres Ortega. Carlos Alberto
//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(); }
//Funcin de movimiento simulado en el Brazo2 en el cual se sigue el mimso patrn 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();
Arana Ruiz. Diego

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, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, y2); }
500, 300, 530, 300); 500, 300, endx, endy); endx, endy, endx + x2, endy + y2); endx + x2, endy + y2, endx + x2 +
500, 300, 530, 300); 500, 300, endx, endy); endx, endy, endx, endy + y2); endx, endy + y2, endx + 15, endy +
} 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)
Arana Ruiz. Diego

{ formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 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, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, 500, 300, 530, 300); 500, 300, 530, 300); 500, 300, endx, endy); endx, endy, endx - x2, endy + y2); endx - x2, endy + y2, endx - x2 +
Arana Ruiz. Diego

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();
//Funcin de movimiento simulado en el Brazo3 en el cual se sigue el mimso patrn 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;
Arana Ruiz. Diego
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, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 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);
Arana Ruiz. Diego

ty2 = Math.Round(ty2); x2 = (int)tx2; y2 = (int)ty2; endx = 600 + x; endy = 300 + y; if (angulo2 < 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 == 45) { formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 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, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, 15, endy + y2); } else if (angulo2 > 45) { formGraphics.DrawLine(myPen, 600, 300, 630, 300);
Arana Ruiz. Diego

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, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, formGraphics.DrawLine(myPen, }
600, 300, 630, 300); 600, 300, endx, endy); endx, endy, endx, endy + y2); 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 CONFIGURACIN:
Esta venta es solamente para poder configurar algunos parmetros como se ha mencionado al inicio de este documento.
//Declaramos las libreras a utilizar

using using using using using using using using System; System.Collections.Generic; System.ComponentModel; System.Data; System.Drawing; System.Linq; System.Text; System.Windows.Forms;
//Espacio de trabajo
namespace Robot_Delta {
//Mi clase principal

public partial class Configuracion : Form {
//Declaracin de variables
Arana Ruiz. Diego
private private private private private private
String String String String String String
h; r; o; b; ha; cuentas;
//Funcin que permite inicializar esta ventana como se muestra en la figura 9.

public Configuracion() { InitializeComponent(); }
Figura 9. //Funcin que permite leer el valor que se ingrese a travs de una caja de texto de manera dinmica y que estos valores se puedan utilizar posteriormente en la ventana principal.
public String H { get { return h; } set { h = value; } }
Arana Ruiz. Diego

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 botn aceptar donde se igualan las variables ledas a travs 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 botn Parametros predeterminados, que pone en la caja de texto los parmetros de un robot en especfico para evitar la tarea de estar ingresando cada uno de ellos. Observar figura 10.
Arana Ruiz. Diego

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

Manual de Operaciones Robot Delta

Cargado por

Información del documento

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Manual de Operaciones Robot Delta

Cargado por

Copyright:

Formatos disponibles

Universidad Autnoma de Quertaro Facultad de Ingeniera

DISEO E IMPLEMETACIN DE HARDWARE.

DISEO ELECTRNICO Figura 1. Arquitectura del sistema.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

Figura 2. Arquitectura y distribucin geomtrica de un brazo delta.

DISEO ELECTRNICO. Alimentacin.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

Fuente CPU. 5V, 12V, -12V Fuente 24VDC 10 Amperes.

Figura 3. Fuente Simtrica de computadora y fuente de 24VDC a 10 A.

Figura 4. Circuito de distribucin de carga.

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

Y le corresponden estas caractersticas:

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

Universidad Autnoma de Quertaro Facultad de Ingeniera

A continuacin se detalla la tarjeta que contiene al PIC.

LED indicador de alimentacin

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

Amplificador Alimentacin DAC REFERENCIA servodriver

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

//identificador del PIC/MOTOR

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

//Transferencia de salida PID

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

min = 0.0; max = 255.0; posRef = 500.0;

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

Aguilar Hdez. Gustavo

Arana Ruiz. Diego

Torres Ortega. Carlos Alberto

Universidad Autnoma de Quertaro Facultad de Ingeniera

Aguilar Hdez. Gustavo