Diseño, implementación y control de una pinza servocontrolada para un exoesqueleto

UNIVERSIDAD POLITÉCINA DE VALENCIA Departamento de Ingeniería de Sistemas y Automática (DISA) MÁSTER UNIVERSITARIO EN AUTOMÁTICA E INFORMÁTICA INDUSTRIAL Diseño, implementación y control de una pinza servo-controlada para un exoesqueleto TRABAJO DE FIN DE MÁSTER AUTOR: Camacho Muñoz, Jefferson Fernando TUTOR: Zotovic, Ranko VALENCIA - ESPAÑA 2016 Índice general Índice de figuras iv Índice de tablas vii 1. ALCANCE DE LA INVESTIGACIÓN 2 1.1. Objetivos ............................................. 2 1.1.1. Objetivogeneral ..................................... 2 1.1.2. Objetivosespecíficos................................... 2 1.2. Justificación............................................ 2 1.3. Metodología............................................ 3 2. ESTADO DEL ARTE 6 2.1. Introducción............................................ 6 2.2. Robótica.............................................. 7 2.3. Sistemarobóticomultimodal .................................. 9 2.4. Exoesqueletos........................................... 13 2.4.1. Clasificación de los exoesqueletos . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 2.4.1.1. Exoesqueletos No portátiles . . . . . . . . . . . . . . . . . . . . . . . . . . 15 2.4.1.2. Exoesqueletos Portátiles . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.4.2. Aplicaciones de los exoesqueletos robóticos . . . . . . . . . . . . . . . . . . . . . . 16 2.4.2.1. Amplificadores de potencia . . . . . . . . . . . . . . . . . . . . . . . . . . 17 2.4.2.2. Dispositivo para realimentación háptica y de fuerzas . . . . . . . . . . . . 20 2.4.2.3. Rehabilitación y entrenamiento motor . . . . . . . . . . . . . . . . . . . . 21 2.4.2.4. Estudio e investigación . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22 2.4.3. Power Asist suit Kawassaki . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.5. EfectorFinal ........................................... 25 2.5.1. Herramienta ....................................... 25 2.5.1.1. Criterios de selección de manipuladores . . . . . . . . . . . . . . . . . . . 26 2.5.2. PinzaRobótica...................................... 27 2.6. Pinzarobóticacomoprótesis .................................. 29 i ÍNDICE GENERAL 3. MATERIALES Y MÉTODOS 32 3.1. Hardware ............................................. 32 3.2. Fundamentos para el diseño de la pinza . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 3.2.1. Puntosdecontacto.................................... 32 3.2.2. Número de dedos a utilizar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 3.2.3. Fuerza de acuerdo a los puntos de presión . . . . . . . . . . . . . . . . . . . . . . . 34 3.2.4. Desplazamiento para el agarre . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 3.2.4.1. Áreaaocupar ................................. 37 3.2.4.2. MáximodeApertura ............................. 37 3.2.4.3. DesgasteMecánico............................... 38 3.2.4.4. FuerzaMáxima ................................ 38 3.2.4.5. Objetosasujetar ............................... 39 3.2.5. Tamañodelosdedos................................... 40 3.3. Diseñodelapinza ........................................ 40 3.3.1. Carainferiordelapinza................................. 41 3.3.2. Carasuperiordelapinza ................................ 42 3.3.3. Componente izquierdo y derecho compuesto por dos dedos cada uno . . . . . . . . 43 3.3.4. Ejesderotación ..................................... 45 3.4. SensoresdeFuerza........................................ 46 3.5. Diseño de circuitos electrónicos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 3.6. Software.............................................. 50 3.6.1. Arquitectura de programación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 3.6.1.1. InterfazUSB.................................. 53 3.6.1.2. SDRAM .................................... 53 3.6.2. Identificación del sistema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 3.6.2.1. Filtro del sensor de retroalimentación . . . . . . . . . . . . . . . . . . . . 55 3.6.2.2. Obtenerdatos ................................. 57 3.6.2.3. Calculo de la función de transferencia . . . . . . . . . . . . . . . . . . . . 58 3.6.2.4. Simulación y validación de las ecuaciones obtenidas . . . . . . . . . . . . 61 3.6.2.5. Diseño de controlador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63 3.6.3. Control .......................................... 66 3.6.3.1. Control por Velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 3.6.3.2. Control por Posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70 3.6.3.3. ControlporFuerza .............................. 75 ii ÍNDICE GENERAL 4. FUNCIONAMIENTO Y PRUEBAS 82 4.1. ConexiónTarjetastm32f4xx................................... 82 4.1.1. GPIOautilizar...................................... 82 4.2. Libreriasautilizar ........................................ 84 5. CONCLUSIONES Y RECOMENDACIONES 88 5.1. Conclusiones ........................................... 88 5.2. Recomendaciones......................................... 90 Bibliografía 91 ANEXOS 94 A. Cotas de la pinza 95 B. Librerías Usadas 99 B.1.USB ................................................ 99 B.2.ADC................................................ 100 B.3.PWM ............................................... 100 B.4.SDRAM.............................................. 100 B.5.TIMER .............................................. 100 B.6.Libreriasgenerales ........................................ 101 B.7. Librerias creadas para el presente proyecto . . . . . . . . . . . . . . . . . . . . . . . . . . . 101 C. Esquemas para circuito de sensores 102 D. Código desarrollado 104 D.1.main.c............................................... 104 D.2.accions.c.............................................. 111 D.3.accions.h.............................................. 117 iii Índice de figuras 1.1. Etapas de elaboración del proyecto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 2.1. Crecimiento de la automatización robotizada[3] . . . . . . . . . . . . . . . . . . . . . . . . 7 2.2. Diagrama de bloques del sistema neuromuscular-esquelético humano[30] . . . . . . . . . . 10 2.3. Esquema en lazo cerrado del sistema de control[31] . . . . . . . . . . . . . . . . . . . . . . 11 2.4. Configuración del exoesqueleto robótico para estudios del movimiento del miembro superior. Izq: Esquema del movimiento; Der: Dispositivo adaptado sobre el miembro superior[30] 12 2.5. TarjetaRaspberryPi3...................................... 13 2.6. Encoderconsensoróptico.................................... 13 2.7. Baterías montadas en exoesqueleto[9] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.8. Marcibionics Exoesqueleto mecanismo de movilidad[10] . . . . . . . . . . . . . . . . . . . . 14 2.9. Marcodeexoesqueleto...................................... 15 2.10. Clasificación en base a la portabilidad (a) No portátiles, (b) Portátiles . . . . . . . . . . . 16 2.11.ExoesqueletoHULC[14] ..................................... 18 2.12. Sistema teleoperado master-esclavo[15] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.13. Interfaz Háptica Cybergrasp[16] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 2.14. Exoesqueleto para rehabilitación[17] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 2.15. Diseño biomecánico de una prótesis robótica[20] . . . . . . . . . . . . . . . . . . . . . . . . 22 2.16. Brazo accionado de forma neumática Kawassaki[29] . . . . . . . . . . . . . . . . . . . . . . 24 2.17. Esquema de traje Kawassaki[29] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.18. Exoesqueleto desarrollado por la empresa Kawassaki . . . . . . . . . . . . . . . . . . . . . 25 2.19. Efectores finales utilizados frecuentemente . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 2.20. Grados de libertad en una mano humana . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 2.21.Doblepinzabasculante[24].................................... 29 2.22.Protesisdemanoscompletas .................................. 29 3.1. Formas de objetos posibles[32] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 3.2. Puntos de aplicación de fuerza . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 3.3. Desplazamientos Angular y Rectilíneo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 3.4. Dimensionesdepiñones[34] ................................... 41 iv ÍNDICE DE FIGURAS 3.5. Carainferiordelapinza..................................... 42 3.6. Carasuperiordelapinza .................................... 43 3.7. Componentes Izquierdo o derecho con sus respectivas partes . . . . . . . . . . . . . . . . . 44 3.8. Unión entre los dedos y los ejes de la cara inferior . . . . . . . . . . . . . . . . . . . . . . . 45 3.9. Unión entre los dedos y los ejes de la cara superior . . . . . . . . . . . . . . . . . . . . . . 45 3.10.SensordeFuerza ......................................... 46 3.11.DivisordeVoltaje[35]...................................... 46 3.12. Conjunto de pesas para pruebas de sensor de fuerza . . . . . . . . . . . . . . . . . . . . . 47 3.13. Salida - Peso aplicado Resistor 10 kΩ.............................. 48 3.14. Salida - Peso aplicado Resistor 1 kΩ.............................. 48 3.15. Salida - Peso aplicado Resistor 2 kΩ.............................. 49 3.16. Diseño de placa con entradas y salidas de sensores de fuerza . . . . . . . . . . . . . . . . . 49 3.17.Esquemainversordevoltaje................................... 50 3.18. Esquema de conexión puente h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 3.19. Arquitectura de control general . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 3.20. Maquina de estados para control general . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 3.21. Maquina de estados para obtención de datos . . . . . . . . . . . . . . . . . . . . . . . . . . 52 3.22.DatosObtenidossinfiltro .................................... 54 3.23. Datos Obtenidos con filtrado mediante capacitor electrolítico . . . . . . . . . . . . . . . . 55 3.24. Filtro promediador con 30 muestras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56 3.25. Filtro promediador con 50 muestras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 3.26. Sistema de experimentación con ganancia 3 en el proporcional . . . . . . . . . . . . . . . . 58 3.27. Diseño en simulink para el esquema en bucle cerrado . . . . . . . . . . . . . . . . . . . . . 62 3.28. Esquema real y esquema simulado con la función de transferencia obtenida . . . . . . . . 62 3.29. Esquema en bucle abierto y en bucle cerrado . . . . . . . . . . . . . . . . . . . . . . . . . 63 3.30. Comparación en bucle abierto y en bucle cerrado . . . . . . . . . . . . . . . . . . . . . . . 63 3.31. Diseño del controlador PD Sisotool . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 3.32. Respuesta al impulso mediante Sisotool . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 3.33. Respuestas ante referencia con controlador PD y P . . . . . . . . . . . . . . . . . . . . . . 65 3.34. Esquema de control para Velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 3.35. Esquema de control por estados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 3.36. Esquema de funcionamiento para Velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . 67 3.37. Ruido en el sensor del motor para determinar la velocidad del mismo . . . . . . . . . . . . 68 3.38. Desplazamiento constante para determinar la velocidad . . . . . . . . . . . . . . . . . . . 69 3.39. Esquema de control para Posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71 3.40. Esquema de funcionamiento para Posición . . . . . . . . . . . . . . . . . . . . . . . . . . . 72 3.41.EsquemadecontrolPID[38]................................... 73 3.42.AlgoritmodecontrolPID .................................... 74 v ÍNDICE DE FIGURAS 3.43. Apertura de la pinza dentro del rango que va de 115 a 145 grados . . . . . . . . . . . . . . 75 3.44. Esquema de funcionamiento para Fuerza . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76 3.45. Esquema de control para Fuerza . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 3.46. Objeto sujetado con una fuerza equivalente al 80 %de la capacidad total de la pinza . . . 78 3.47. Objeto sujetado con una fuerza equivalente al 90 %de la capacidad total de la pinza . . . 79 3.48. Objeto sujetado con una fuerza equivalente al 50 %de la capacidad total de la pinza . . . 80 4.1. DefinicionesenTarget ...................................... 84 4.2. Algunas de las librerías utilizadas para el proyecto . . . . . . . . . . . . . . . . . . . . . . 84 4.3. PathDiscovery .......................................... 85 4.4. Puerto a usar por la tarjeta Discovery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85 4.5. Posiblesaccionesarealizar ................................... 85 4.6. Forma de ejecutar las diferentes acciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86 4.7. Ingresando mas de un valor para determinar posición . . . . . . . . . . . . . . . . . . . . . 86 A.1. Medidas Cara inferior de la pinza . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96 A.2. Medidas Cara superior de la pinza . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 A.3.Medidasambosdedos ...................................... 98 C.1. Placa acondicionada para sensores de fuerza . . . . . . . . . . . . . . . . . . . . . . . . . . 102 C.2. Top silk para nombrar los diferentes puntos de la placa . . . . . . . . . . . . . . . . . . . . 103 vi Índice de tablas 2.1. CAUSAS COMUNES DE DEBILIDAD MUSCULAR . . . . . . . . . . . . . . . . . . . . 19 2.2. HERRAMIENTAS TERMINALES PARA ROBOTS [22] . . . . . . . . . . . . . . . . . . 25 3.1. Acciones a realizar, necesidad y posibilidad de realizarla . . . . . . . . . . . . . . . . . . . 39 3.2. Velocidad en diferentes instantes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70 4.1. GPIO Disponibles para funcionar como ADC’s . . . . . . . . . . . . . . . . . . . . . . . . 83 vii ALCANCE DE LA INVESTIGACIÓN 2.2 Robótica Trasporte y automoción Medioambiental Misiones de rescate Vigilancia y seguridad Militar Tareas domésticas Entretenimiento y ocio Agricultura y ganadería Educación e información Asistencia y cuidado ancianos Construcción Quirúrgica Rehabilitación Exoesqueletos y prótesis Arte y patrimonio Submarina Existen aspectos de los mencionados anteriormente en los cuales ya se han desarrollado importantes avances en referencia a la tecnología que los precede como en el campo de la agricultura y ganadería cuyo principio fundamental de funcionamiento se basa en el reconocimiento del cultivo mediante imágenes que son procesadas y a su vez envían órdenes precisas en cuanto a las tareas a realizar, con alguna planta del cultivo, En la actualidad, existen pequeños robots que realizan tareas de búsqueda y transmisión de información sobre un determinado terreno de una forma totalmente nueva en el sector agrícola, aunque a futuro se prevé el uso de drones para realizar un recorrido más extendido y más rápido de un determinado entorno[4], estos equipos correspondientes al área aérea y espacial han ido evolucionando en cuanto a alcance, tiempo de vuelo, transmisión de datos y manipulación de los mismos se refiere. También existen importantes avances en la construcción ya que la aplicación de un sistema de automatización en la construcción incrementa la productividad y la calidad de una determinada obra, de concretarse los proyectos en marcha se pretende reducir el tiempo de construcción en un 70%, De esta manera, se garantiza un trabajo más eficiente, así como un incremento de calidad y con prestaciones de las edificaciones acorde con las necesidades de la sociedad actual, Santiago Martínez de la Casa de la 8 2.3 Sistema robótico multimodal Universidad Carlos III de Madrid dice: que el estudio se centra en la introducción de robots industriales comerciales en el entorno de la construcción, favoreciendo el ensamblaje de múltiples productos y que pueden ser transportado de obra en obra[5]. Se podría tambien hablar del campo submarinos cuyos vehículos son aquellos no tripulados sumergibles que pueden ser controlados desde la superficie, pueden ser altamente maniobrables y al menos, llevan una cámara de video y luces. Se selecciona el tipo de robot a usar dependiendo del entorno u objetivo, pueden llevar diferentes accesorios tales como: sonares, brazos manipuladores y sensores de variados tipos. Actualmente hay varias empresas que se dedican a la investigación y desarrollo de este tipo de robots ya que muchos científicos u empresas los utilizan para llevar a cabo sus investigaciones marítimas[6]. Sin duda existen varios avances en todos las líneas de investigación mencionadas en el presente documento pudiendo ser una de las ramas que cuenta con mayor inversión y a la que se le atribuyen grandes avances y la línea militar, los robots militares se remontan a la Segunda Guerra Mundial y la Guerra Fría en la forma de Alemania minas Goliat y la Unión Soviética teletanks, El MQ-1 Predator drone fue cuando agentes de la CIA vieron los beneficios prácticos por primera vez en la fantasía de lo que en ese entonces era, la utilización de robots aéreos para recabar información de inteligencia, no obstante existe quienes creen que el futuro de la guerra moderna se peleará por los sistemas de armas automáticos1, Es por tal motivo que potencias mundiales invierten grandes cifras en su investigación y desarrollo[7]. Parte de estas investigaciones no solo abarcan el desarrollo de armamento bélico, también se investiga la posibilidad de usando estructuras mecánicas controladas, se pueda proveer al ser humano de capacidades de fuerza, precisión, etc, que de otra manera seria imposible adquirirlas, ha estos aparatos mecánicos se les denomina exo-esqueletos, misma denominación que forma parte de una de las líneas de investigación mencionadas anteriormente. Conscientes del potencial que pudiesen aportar los exo-esqueletos, se ha extendido su línea de investigación para dotar a personas que han sido despojadas de movilidad en sus extremidades o por ausencia de ellas, ha pernoctar en sus capacidades para investigar sobre la forma en como sustituirlas, para que el individuo que las posee se reintegre a sus actividades diarias de forma fiable y con la seguridad que solo sus extremidades naturales le fueron capaces de dar un día. 2.3. Sistema robótico multimodal El trabajo de investigación denominado "Sistema Robótico Multimodal para Análisis y Estudios en Biomecánica, Movimiento Humano y Control Neuromotor"desarrollado por <Andrés Felipe Ruíz Olaya>, en la Universidad Carlos III de Madrid2, es orientada a: diseñar, implementar y evaluar un sistema, 1https://en.wikipedia.org/wiki/Military_robot 2http://www.uc3m.es/Inicio 9 2.3 Sistema robótico multimodal compuesto por un exoesqueleto robótico de miembro superior y módulos independientes, que al actuar en conjunto dan una solución multimodal dentro del estudio neuromotor humano, desarrollando tanto el exoesqueleto como diversas herramientas, han sido integradas en un sistema común en esquema multimodal, siendo objeto de estudio la articulación del codo se ha determinado la viabilidad de estimar la impedancia mecánica a partir de la activación de los musculos asociados al movimiento de dicha articulación, específicamente bíceps y tríceps braquial. Desde este punto de vista y en referencia al movimiento humano, existen diversos subsistemas que componen el sistema principal el cual enlaza una señal generada en el sistema nervioso central a un evento de movimiento humano. Teniendo como base que uno de los desafíos que afrontan los investigadores en neurociencia, involucrados en el análisis del movimiento humano, es entender que eventos o secuencia de los mismo, conecta la actividad neuronal a la contracción muscular, producto de lo cual se genera el movimiento humano y la interrelación entre los distintos subsistemas que componen el sistema motor humano para lo cual se plantea el esquema mostrado en la figura 2.2, con el fin de comprender su funcionamiento[30]. Figura 2.2: Diagrama de bloques del sistema neuromuscular-esquelético humano[30] 10 2.3 Sistema robótico multimodal De forma sencilla se puede simplificar el esquema en una entrada producto de un estímulo neurológico produciendo una salida traducida como una activación muscular, desde el punto de vista técnico al cual se acogen otros autores se vería como el esquema de la figura 2.3, en el cual TF F corresponde a la estimación del bloque predictivo o feed foward y el y TF B corresponde al componente de realimentación feedback, resultando las propiedades elásticas del músculo[31]. Este esquema de control compuesto por una serie de bloques anticipativos o predictivos y diversos lazos de realimentación o feedback. Desde la perspectiva de la ingeniería de control, a un sistema de control se le aplica una perturbación externa y se lo cataloga al mismo mediante la observación de los cambios en su dinámica. De esta forma, se podría decir que para las investigaciones del movimiento humano y control neuromotor existen escenarios en los cuales es necesario adquirir y registrar la actividad eléctrica del sujeto, en sincronía con las mediciones cinemáticas y dinámicas producidas por el movimiento humano para modelar diversos aspectos del sistema de control[31]. Figura 2.3: Esquema en lazo cerrado del sistema de control[31] Con base en los puntos anteriores se desarrollo en primera instancia, un dispositivo robótico el cual fuese capaz de aplicar diferentes pares definidos sobre el movimiento voluntario del sujeto, usando soportes para el exoesqueleto los cuales proporcionen una transmisión efectiva de la carga sobre el miembro superior, para lo cual se ha usado materiales termo-plásticos como elementos de sujeción esta etapa concluida se muestra en la figura 2.4. Este dispositivo ha sido diseñado tomando en consideración que se debe mantener un rango de movimientos en un espacio seguro para quien lo porta, ajustando su cinemática al miembro superior y restringiendo el rango de movimientos, a uno menor que el perteneciente al mismo miembro en cada una de sus articulaciones, se ha considerado también que el uso del dispositivo no altere a gran escala los patrones fisiológicos de la persona, ya que los movimientos del dispositivo dependen del umbral de presión de estos, limitando de esta manera los pares máximos a ser aplicados por el dispositivo robótico en las articulaciones y sus zonas de aplicación. 11 2.3 Sistema robótico multimodal Figura 2.4: Configuración del exoesqueleto robótico para estudios del movimiento del miembro superior. Izq: Esquema del movimiento; Der: Dispositivo adaptado sobre el miembro superior[30] El dispositivo robótico diseñado anteriormente sirve como dispositivo de medida y actuación otros elementos como la herramienta de adquisición de EMG1, permiten registrar otras variables de interés asociadas a la actividad bioeléctrica, ya que cuenta con una arquitectura de control abierta, permite ejecutar en tiempo real algoritmos de control desarrollados en MATLAB2, siendo posible la validación del hardware y adquisición de señales del mismo. La experimentación del dispositivo en un entorno controlado a permitido comparar y validar pares generados y aplicados por el exoesqueleto, siendo posible la configuración de una serie de condiciones de operatividad sobre seguimiento de perfiles de fuerza, reproduciendo varios entornos dinámicos y generando perturbaciones mecánicas. De este modo y mediante las aportaciones descritas anteriormente investigación llevada a cabo por Felipe Ruíz, deja constancia de tres componentes detalladas como lo son: científica, tecnológica y experimental en sujetos reales, cuya componente tecnológica se mostró en la figura 2.4, la componente científica implicada en el modelado de la dinámica del codo conjuntamente con esta información y la información EMG de los musculos asociados a dicha articulación. desarrollando de esta forma un sistema robótico para realizar estudios experimentales del sistema neuromotor humano. Este proyecto ha dejado abiertas nuevas líneas de investigación tales como: Estudio del movimiento humano (al nivel del miembro superior) bajo cargas externas, ampliando su utilización a otras articulaciones como la muñeca. Configuración de experimentos que involucren no solo control postural sino también control del movimiento, etc. Estas líneas propuestas entre otras son de las cuales ha partido el presente proyecto. 1http://www.aboutkidshealth.ca/En/HealthAZ/Multilingual/ES/Pages/electromyography-emg.aspx 2http://es.mathworks.com/products/matlab/ 12 2.4 Exoesqueletos 2.4. Exoesqueletos El exoesqueleto no es mas que un aparato mecánico el cual sostiene al cuerpo desde afuera, a diferencia del esqueleto que lo conforman los huesos, el cual sostiene al cuerpo desde adentro; son usualmente diseñados para aumentar las capacidades de las personas como su fuerza, movilidad, resistencia o para ejercer actividades físicas que sin ellos les fuese imposibles. Los exoesqueletos pueden estar compuestos de varios componentes como: Controlador: Parte del equipo de la electrónica del dispositivo, actuará como el cerebro del mismo tomando lecturas proporcionadas por los sensores y tomando como referencia las mismas decidir que acciones deben ejecutar los actuadores, si la lógica y la medición de las mismas es correctas permitirá al usuario pararse, caminar, levantar objetos u otras actividades para las cuales halla sido programado, un posible controlador podría ser la tarjeta Raspberry que se muestra en la figura 2.5 Figura 2.5: Tarjeta Raspberry Pi 3 Sensores: Ya sean de fuerza, visión, distancia u otras variables que se deseen, obtendrán la información del entorno en que se ejecuta su lectura, para posteriormente ser transmitido al controlador, uno de estos sensores podría ser el encoder el cual conjuntamente con un sensor óptico, figura 2.6, entregan la información correspondiente al número de vueltas de un motor eléctrico. Figura 2.6: Encoder con sensor óptico 13 2.4 Exoesqueletos Baterías: Teniendo como función principal el movimiento del exoesqueleto, deben ser capaces de portar la energía suficiente para su movimiento y energizar los componentes eléctricos o electrónicos que pueda contener, estas deben recargarse rápidamente y no pesar demasiado para que el exoesqueleto no sea voluptuoso o demasiado pesado como es el caso de la figura 2.7. Figura 2.7: Baterías montadas en exoesqueleto[9] Actuadores: Son los que se encargarán de los movimientos del exoesqueleto siendo usualmente de tipo eléctrico o hidráulico, estos ejecutan sus movimientos usando la energía enviada por las baterías y la información transmitida por el controlador, llevando el exoesqueleto a posiciones precisas y sin demasiado esfuerzo para la persona que los usa este tipo de dispositivos podrían lucir como el de Marcibionics1que se muestra en la figura 2.8. Figura 2.8: Marcibionics Exoesqueleto mecanismo de movilidad[10] Marco: El marco debe de ser de un material ligero y resistente como se muestra en la figura 2.9, ya que va a soportar no solo el peso de los objetos que vaya a levantar sino también el peso del sujeto que lo utilice, no debe suponer mayor esfuerzo y debe ser seguro para quien lo porte a fin de que no se lastime o sufra algún accidente relacionado con el uso del exoesqueleto[8]. 1http://www.smartherapy.org/crowdfunding-marsi-bionics/ 14 2.4 Exoesqueletos Figura 2.9: Marco de exoesqueleto A diferencia de las estructuras que comprende el diseño de un exoesqueleto completo, existen también exoesqueletos diseñados para partes específicas del cuerpo ya sea un brazo una pierna o la mano, siendo estas usadas por quienes carecen de la extremidad o porque han perdido la movilidad del mismo, estos diseños aparte de las exigencias de potabilidad, también es necesario que sean precisos, ya que interactuarán de manera directa con su portador y con el entorno que los rodea, cumpliendo tareas sencillas para el cuerpo humano pero de mayor complejidad para una extremidad robótica; como es el caso de levantar un recipiente que contenga líquido y verterlo en otro, su complejidad es mayor ya que quien lo manipula se limita a su necesidad y la extremidad robótica mediante sus diferentes sensores, debe interactuar no solo con el entorno sino también con las decisiones de la persona que lo porta, este ámbito es el que se encuentra actualmente en fase de estudio aunque ya se han tenido grandes avances. 2.4.1. Clasificación de los exoesqueletos Los exoesqueletos se pueden clasificar en funcion de los actuadores utilizados(neumático, eléctrico o hidráulico), cadena cinemática, tipo de control, o aplicación, los diseños actuales en su mayoría se basan en interfaces sujetas a una base externa, pudiendo ser clasificados como portátiles o no portátiles, esta ultima clasificación se la puede apreciar en la figura 2.10. 2.4.1.1. Exoesqueletos No portátiles A diferencia de los portátiles estos se encuentran anclados a una base fija, pudiendo ser esta el suelo, la pared o el techo, ya que el peso no recae en el usuario, este tipo de exoesqueletos suele ser mucho mas pesado y a su vez más complejas, ya que pueden contar con mayor número de actuadores o instrumentos capaces de generar un torque mayor debido a la robustez de su estructura, siendo uno de los puntos mas considerados la seguridad que puedan ofrecer los mismos (véase figura 2.10 a). 15 2.4 Exoesqueletos Ya que no están restringidos en peso o volumen se puede utilizar cualquier dispositivo sobre los mismos, inclusive transmisiones mecánicas como cables o poleas, siendo posible el uso de herramientas tradicionales para las diferentes transmisiones de fuerza[11]. 2.4.1.2. Exoesqueletos Portátiles Es un dispositivo mediante el cual el usuario tiene que ser quien soporte el peso del exoesqueleto o un dispositivo móvil como una silla de ruedas que se mueva conjuntamente con el sujeto, de esta forma los puntos de presión o fuerza son ejercidos sobre el usuario en los puntos de sujeción (véase figura 2.10 b). Ya que existen restricciones de peso, se inhibe el uso de un gran numero de actuadores, pero también se reduce la cantidad de grados de libertad para poder ejercer fuerzas de forma apropiada sobre los miembros del sujeto, es por tal motivo que los exoesqueletos portátiles requieren especial atención en el número de grados de libertad[11].. Este tipo de características en particular obliga a quien lo diseñe a crearlo con menos peso y volumen, evitando de esta manera la fatiga muscular, por ende sus actuadores deben corresponderse en potencia y peso o potencia y volumen para que puedan ser usados en este tipo de interfaces, siendo clasificados también en dos grupos exoesqueletos para el brazo o para la mano. Figura 2.10: Clasificación en base a la portabilidad (a) No portátiles, (b) Portátiles 2.4.2. Aplicaciones de los exoesqueletos robóticos Aunque los exoesqueletos han sido utilizados en diferentes aplicaciones. En forma de concepto estos sistemas se pueden utilizar en las siguientes aplicaciones. Amplificadores de potencia. •Asistencia y compensación funcional. 16 2.4 Exoesqueletos Dispositivo para re-alimentación háptica y de fuerzas •Dispositivos de un sistema de teleoperación maestro/esclavo. •Dispositivos hápticos. Rehabilitación y entrenamiento motor. Estudio e investigación. •Biomecánica. •Neurofisiología y control motor. •Trastornos de movimiento. 2.4.2.1. Amplificadores de potencia Como su nombre lo indica, son capaces de amplificar la capacidad física del sujeto que lo porta, aunque también puede ser usado por personas que hayan sufrido algún tipo de accidente el cual limite sus capacidades físicas o motoras[13]. Un estudio llevado a cabo por la Universidad de Pamplona1acerca de las Órtesis2activas, establece los exoesqueletos son de gran ayuda durante la terapia ya que permiten interactuar directamente con el humano emulando sus movimientos, esto se debe a que lo conforma un mecanismo cuya estructura esta acoplado externamente y sus juntas y eslabones corresponden a las partes del cuerpo humano que simula, el contacto entre estos sistemas y el cuerpo humano permite la transferencia de potencia mecánica y señales de información[12]. Cuando funcionan como amplificadores de potencia, el usuario transmite señales al exoesqueleto, ya sea mediante estímulos a sensores de diferente tipo o por tareas que fuesen pre-definidas teniendo como fin una determinada tarea, el usuario que utiliza este sistema puede desarrollar pesadas tareas de índole físico con el mínimo esfuerzo. Un ejemplo notable en el desarrollo de este tipo de exoesqueletos es el desarrollado por los EE:UU para sus soldados el denominado HULC. Exoesqueleto HULC: El Human Universal Load Carrier (HULC) o portador de carga universal humano es como la empresa "Lokheed Martin"denominó al exoesqueleto desarrollado para las fuerzas militares de los EE:UU, en una serie de pruebas en conjunto con Natick Soldier Research Development and Engineering Center(NSRDEC) el potencial de los exoesqueletos para aliviar el esfuerzo y la fatiga de 1http://www.unipamplona.edu.co/ 2http://ortopedia1.com/ortesis 17 2.4 Exoesqueletos Figura 2.16: Brazo accionado de forma neumática Kawassaki[29] En el año de 1994 la misma empresa desarrollo un traje de potencia compuesto por ambos brazos piernas y cintura basados en el mismo principio usado anteriormente mediante actuadores de goma, los brazos, cintura y las piernas estaban conectadas entre si y de la misma manera estaban enlazados al usuario, dándole el aspecto de traje cuyo esquema se muestra en la figura 2.17. Figura 2.17: Esquema de traje Kawassaki[29] En los diseños primarios el traje había estado sujeto a cables para su alimentación y mangueras para el suministro de aire, hasta que en el año 2002 realizaron una disminución considerable del gasto de energía y de los sistemas de suministro de aire logrando que estos sean portables, Los brazos, las piernas y la cintura tienen actuadores rotativos neumáticos, accionados directamente con bombas de aire micro suministrados por baterías portátiles de níquel, las fuerzas musculares se detectan con un nuevo sensor de dureza del músculo que utiliza una punta sensora montada en un dispositivo de película gruesa de polímero, el microordenador incorporado calcula el par necesario para mantener una posición de acuerdo con las ecuaciones derivadas de la mecánica del cuerpo; utilizando los ángulos de la articulación y el par necesario, se combina con las señales de salida para crear las órdenes de control. En los años próximos iría evolucionando a tal punto que en el año 2005 la empresa contaba con un traje mucho mas robusto, de esta forma, a avanzado continuamente logrando en los últimos años, diseñar un exoesqueleto acorde a los objetivos planteados, quedando tal como se observa en la figura 2.18. 24 2.5 Efector Final Figura 2.18: Exoesqueleto desarrollado por la empresa Kawassaki 2.5. Efector Final En las aplicaciones industriales para completar el trabajo de un robot, se debe utilizar dispositivos adicionales, los cuales se podría denominar como periféricos del robot, El termino para denominar al dispositivo que esta unido a la muñeca dentro de robótica es actuador final, esta herramienta esta diseñada especialmente para hacer que el robot de propósito general cumpla con una tarea en específico[22]. Los efectores finales pueden dividirse en dos grandes grupos: Herramienta Gripper(Pinza) 2.5.1. Herramienta Las herramientas utilizadas como efectores finales, están diseñadas para cumplir con una tarea determinada en la cual deba aplicarse una operación sobre las piezas de trabajo algunas de estas herramientas y su función se indican en la tabla 2.2. En cada uno de estos casos la pieza trabaja unida a la muñeca para llevar a cabo el trabajo asignado. Herramienta Función que desarrolla Soldadura por punto Dos electrodos que se cierran sobre la pieza a soldar Soplete soldadura de arco Aportan el flujo de electrodo el cual se funde Cucharón para colada Se utiliza en trabajos de fundición Atornillador Suelen incluir la alimentación de tornillos Fresa-lija Para perfilar, eliminar rebabas, pulir, etc Pistola de pintura Para pulverización de la pintura Cañón láser Para corte de materiales, soldadura o inspección Cañón de agua a presión Para corte de materiales Tabla 2.2: HERRAMIENTAS TERMINALES PARA ROBOTS [22] 25 2.5 Efector Final Algunos de los manipuladores se pueden apreciar en las imágenes de la figura 2.19. Figura 2.19: Efectores finales utilizados frecuentemente 2.5.1.1. Criterios de selección de manipuladores Existen varios criterios de selección para los manipuladores entre los cuales los más usados son: Trabajo de la pieza al ser manejada.- En función del robot en el cual se utilizará la pieza se definen las características de la misma tales como: •Dimensiones •Masa •Potencia •Geometría de robot Actuadores Los actuadores pueden ser de diferentes características dependientes de las piezas con las que se va a trabajar pudiendo ser de tipo: •Mecánico •Al Vacío •Magnético Materiales En función del entorno y piezas en las cuales se va a trabajar se puede definir el material del efector final 26 2.5 Efector Final •Fuerte, rígido, durable •Esfuerzo continuo •Costo y factibilidad en producción •Coeficiente de fricción •Entorno de trabajo Posicionamiento En aplicaciones industriales es importante que exista repetibilidad en la cadena de producción por lo cual el efector final se elige dependiendo de: •Longitud del manipulador •Longitud del robot y repetibilidad •Tolerancias De forma tradicional se puede decir según [23] que los criterios de selección de una herramienta son: 1. Un Peso bajo para permitir tener un manejo de carga más útil, aumento de las aceleraciones, disminución en tiempo de ciclo. 2. Dimensiones mínimas dispuestas por el tamaño de la pieza de trabajo. 3. Rango más ancho de las partes de la pieza usando inserciones, y movimientos ajustables. 4. Rigidez para mantener la exactitud del robot y reducir vibraciones. 5. Fuerza máxima solicitada, seguridad y prevención de daño en los productos. 6. La fuente de poder debe estar prontamente disponible para el robot. 7. El mantenimiento debe ser fácil y rápido. 8. Formas de seguridad para que el material no se deje caer cuando falte la fuente de poder. Mediante estos criterios de selección en nuestros efectores finales podemos adaptar nuestro mecanismo a las funciones que deseemos, es por tal motivo considerar estos criterios entre otros que se puedan presentar al momento de seleccionar el efector final con el cual se va a trabajar. 2.5.2. Pinza Robótica También conocidas como efector final en un brazo robótico; es un sistema cuya principal función es sujetar objetos para obtener un determinado efecto, es importante que las pinzas que se desarrollen puedan moverse en tres dimensiones a fin de que se imite la movilidad de una mano real. Considerando los grados de libertad, una pieza robótica puede tener un máximo de seis grados de libertad, expresados en tres movimientos lineales en las tres dimensiones de los ejes x, y, z y tres movimientos giratorios α1,α2yα3, sobre los ejes x, y, z. Se puede considerar que existen máquinas que 27 2.5 Efector Final cuentan con más de 6 grados de libertad, a las cuales se les conoce como grados de libertad mecánica o de libertad de movimientos[24], Según Bejcy1una mano humana puede ejecutar movimientos con seis grados de libertad tal como se muestra en la figura 2.20. Figura 2.20: Grados de libertad en una mano humana Los movimientos de empuje se describen como: 1. Vertical, arriba/abajo 2. Frontal, avance/retroceso 3. Lateral, izquierdo/derecho De otra manera siguiendo la terminología de aviación según [25] se describen como: 1. α1Cabeceo, inclinación 2. α2Balanceo, torsión 3. α3Guiñada, giro Es importante tener en cuenta que los dedos de la pinza no son considerados como un grado de libertad debido a que su movimiento no interviene en la trayectoria de la pinza. Los sistemas de pinzado pueden ser de diferentes formas dependiendo del elemento u objeto que se desee sostener o con el cual se vaya a laborar; un ejemplo de esto son las diferentes pinzas industriales que se elaboran para un determinado uso, tal es el caso de la figura 2.21, en el cual se ha colocado dos pinzas estándar sobre un placa basculante en la cual un cilindro neumático es el que se encarga del movimiento, el cambio de pinza se hace de forma rápida y se amortigua en el final del recorrido. Este tipo de soluciones suele ser utilizada en equipos de verificación. 1https://www-robotics.jpl.nasa.gov/news/newsStory.cfm?NewsID=117 28 2.6 Pinza robótica como prótesis Figura 2.21: Doble pinza basculante[24] 2.6. Pinza robótica como prótesis Se ha llevado a cabo diferentes estudios y diseños de prótesis mecánicas como [26] [27] para sustituir dedos o la mano de un ser humano, tomando en consideración que en los mismos no se considera el peso de la misma ya que se considera que parte de la mano o en su defecto el resto del miembro superior(brazo) se encuentra completo. Basados en el principio de reconstrucción total de la mano se ha desarrollado prótesis como las que se muestran en la figura 2.22, la figura (a) ha sido desarrollada por ottobock1, la cual puede sujetar objetos de diferente tamaño ya sean frágiles o robustos, esto se debe a que está combinada con prótesis de brazo con control mioeléctrico. Figura 2.22: Protesis de manos completas Su característica especial reside en que las manos pueden abrirse para reaccionar rápidamente en tiempo real, de forma similar a como lo hace una mano natural. Esto le brinda espontaneidad y la capacidad 1http://www.ottobock.es/ 29 2.6 Pinza robótica como prótesis de usar su mano protésica de forma casi natural. Característicamente las prótesis mioeléctricas desarrolladas por ottobock reaccionan normalmente a las respectivas señales musculares del usuario. Las manos SensorHand Speed y VariPlus Speed de la misma empresa disponen de hasta seis programas de control que permiten adaptar la prótesis de mano a las necesidades particulares del usuario, dependiendo de si el usuario puede controlar una o dos señales musculares. Su capacidad de control permite ajustar la fuerza y la velocidad de agarre [28]. En la Figura 2.22 (b), se muestra la mano desarrollada por la Universidad Manuela Beltrán, el prototipo cuenta con 5 grados de libertad que al igual que la mano de la figura 2.22 (a), se controla a través de señales mioeléctricas, las cuales se emiten al momento que el musculo del antebrazo se contrae, logrando de esta forma movimientos sencillos y básicos; para lograr el control y manipulación de la misma se necesita de un proceso de adaptación mediante terapias. El desarrollo de estos prototipos esta orientado a la existencia del brazo y ante brazo en la persona, lo cual hace posible la lectura de señales mioeléctricas para el control de movimientos. 30 MATERIALES Y MÉTODOS 3 MATERIALES Y MÉTODOS 3.1. Hardware Para el diseño e implementación de la pinza tanto en la parte mecánica como electrónica se ha utilizado las siguientes herramientas: Solid Works1 Meshmixer2 Labcenter Electronics(Proteus y Ares)3 El software Solid Works, se utilizó para crear un diseño en 3D de la pinza el cual posteriormente se lo tendría en físico mediante la impresión 3D, este procedimiento se lo lleva a cabo con el objetivo de variar el diseño he ir mejorando cuantas veces sea necesario hasta obtener un prototipo lo más óptimo posible, en tanto que el software de Labcenter, se utilizó para diseñar los circuitos eléctricos a los cuales irán conectados, tanto los sensores de fuerza como el servomotor. 3.2. Fundamentos para el diseño de la pinza El diseño de la pinza se basó en las formas y pesos de objetos que se quería ha llegar a sujetar con la misma, ya sea desde sujetar una cuchara, botella, vaso u otro tipo de objetos habituales. 3.2.1. Puntos de contacto Cada objeto tiene su forma pero el diseño de la pinza no se centra en un objeto en particular a diferencia de ello se basará en varias formas, pudiendo ser: planas, redondas, esféricas, cilíndricas, cúbicas, etc(vease figura 3.1 ). Es decir la pinza debe tener una forma general que englobe las diferentes presentaciones de los objetos a sujetar. 1http://www.solidworks.es 2http://www.meshmixer.com 3https://www.labcenter.com 32 3.2 Fundamentos para el diseño de la pinza Figura 3.1: Formas de objetos posibles[32] Otro de los factores a tener en cuenta es la fuerza que se va a aplicar, cantidad de sensores con los cuales se va a contar y región donde van a estar ubicados los mismos, teniendo en cuenta las diferentes formas que puede haber, en la figura 3.1, se puede apreciar los diferentes puntos de contacto que puede proporcionar una pinza teniendo en cuenta su diseño. A efectos de puntos de presión considerados, se podría decir que la mejor forma de ajustar un objeto sería la forma <E>tanto para el objeto cubico, cilíndrico y esférico sin embargo este tipo de agarre sería perjudicial al momento de medir la fuerza que se aplique a un objeto, ya que si se toma de ejemplo un bolígrafo este quedaría perfectamente ajustado al contorno de las puntas de la pinza con la forma <E>de la figura 3.1, pero si se tomase un objeto de un diámetro mayor o el área de contacto sea mayor que el espacio entre los puntos de presión, se vería afectado el contacto del objeto con el sensor de fuerza, lo cual podría provocar error en las mediciones del sensor o inclusive la inexistencia de variación en las mediciones que se tome del mismo. Es por tal motivo que la mejor opción que se podría tomar en torno a la forma de la punta de los dedos y lugares donde fuese a existir sensores de fuerza, es la <C>de la figura 3.1, la ventaja de esta forma es que al poseer una cara plana puede tomar cualquier tipo de objeto aplicando la misma presión en cada uno de ellos, sin importar su área de contacto debido a que este diseño se implementa con un único punto de contacto. 3.2.2. Número de dedos a utilizar 1. Si se llegase a utilizar un único dedo (véase figura 3.2 A), sería necesario un objeto inmóvil al cual se fuese a trasmitir la fuerza aplicada, dando como resultado una limitación al tamaño y la forma de los objetos que la pinza pudiese sostener, a la fuerza que se pudiese aplicar sobre los mismos y las dimensiones que el objeto o dedo inmóvil debiese tener para asegurar la estabilidad del objeto. 33 3.3 Diseño de la pinza Peso que soportaría la pinza Fuerza necesaria para realizar la actividad No generación de singularidades por parte del brazo Contacto directo con el cuerpo u otro ser humano Considerando que se han analizado todos los puntos hasta ahora expuestos, ventajas y desventajas de cada desplazamiento se ha decidido por utilizar un desplazamiento angular, el cual fijara su movimiento utilizando el servo motor como eje de rotación, proporcionando un movimiento como se muestra en las figuras 3.3 C y D. 3.2.5. Tamaño de los dedos El tamaño de los dedos esta en función de los objetos que se pretenda tomar con el mismo o de las actividades que vaya a realizar; en el capítulo anterior se mostró las actividades que va a realizar la pinza lo cual dará una perspectiva de el tipo de objetos que va a sujetar y de la proporción y peso de estos. Ya que entre todos los objetos el más grande pudiese ser una botella de 2 Litros, la pinza tiene que ser capaz de abrirse lo necesario para tomar este objeto y que al momento de cerrarse tenga la suficiente fuerza para sostener el objeto, y pudiese ser manipulado. 3.3. Diseño de la pinza El diseño e impresión del prototipo permitirá evaluar los planteamientos anteriormente hechos, dando la posibilidad de realizar varias pruebas o re-diseño en caso de que este sea herrado en un principio. Para el diseño en tres dimensiones de la pinza se utilizó el software Solid works en el cual se han construido las piezas por separado y mediante la impresión 3D se ha unido todo para obtener el prototipo. El diseño esta compuesto de las siguientes partes: Cara inferior de la pinza Cara superior de la pinza Componente Izquierdo y derecho compuestos por dos dedos cada uno Ejes de rotación 40 3.3 Diseño de la pinza 3.3.1. Cara inferior de la pinza La cara inferior de la pinza es donde irán asentados tanto el circuito electrónico para los sensores y los ejes de rotación. Este objeto que forma parte de la pinza esta dispuesto con cinco agujeros, de los cuales; tres serán utilizados por los pernos que sujetarán la cara superior con la inferior, y, los dos centrales que servirán como eje de rotación, la distancia entre un eje de rotación y el otro esta determinada por el tipo de piñones que se va a utilizar y la profundidad de la parte dentada de estos piñones. Ya que uno de los objetivos de hacer este tipo de pinza era hacerla ligera y el área ocupada por la estructura para su movimiento fuese lo más pequeña posible; se ha utilizado piñones de 25,6 mm de diámetro cuyas dimensiones se muestran en la figura 3.4 Figura 3.4: Dimensiones de piñones[34] Las dimensiones y composición del componente se corresponden de la siguiente manera: Número de dientes = 30 A = 25,6 mm B = 24 mm D = 12 mm F = 9 mm G = 4 mm Ya que la distancia de B corresponde al punto medio de un diente de un extremo al medio de su contrapuesto, por lo tanto matemáticamente se obtiene que la distancia entre el punto medio del eje de un piñón y el otro es igual a 24 mm, quedando unidos de tal forma que no se produzca un rozamiento descontrolado al momento de ejecutar el movimiento de los dedos o se pueda ver afectado la estructura 41 3.3 Diseño de la pinza del piñón con el uso cotidiano. Conocidos los sitios donde estarán ubicados los ejes del movimiento se determina las dimensiones de la cara inferior sumando las áreas de los piñones más las áreas de los pernos y una perforación la cual servirá de guía para que no desvíen su trayectoria los dedos cuando se estén moviendo, quedando la pieza como se muestran en la figura 3.5 Figura 3.5: Cara inferior de la pinza 3.3.2. Cara superior de la pinza Sobre la cara superior de la pinza se ha posicionado el servo motor que se encarga del movimiento de los dedos, también está ubicado uno de los ejes de movimiento, el segundo eje corresponde al motor, de la misma forma que en la cara inferior cuenta con perforaciones para los pernos que aseguran la estabilidad de la pinza. Para evitar que exista incremento del espesor en la estructura de movimiento de la pinza, se ha resaltado los puntos de agarre del servomotor, y se ha disminuido el espesor en la superficie del objeto, quedando el espacio necesario para el servomotor y los agujeros para que atraviese el piñón de movimiento correspondiente al mismo. Las dimensiones de la cara superior en cuanto a su espesor, ancho y alto son las mismas de la cara inferior y los agujeros centrales son céntricos con la misma, también contará con los agujeros que servirán de guías para el movimiento de los dedos. 42 3.3 Diseño de la pinza Ya que va a realizarse impresión 3D para el modelado físico del prototipo la estructura debe tener en todo momento soportes para evitar deformaciones , es por este motivo que en los sitios donde va a estar ubicado el servomotor existen una especie de ramificaciones, estos son soportes creados por el programa meshmixer para realizar una correcta impresión del prototipo, de no utilizar los mismos la impresión no sería posible. Tomando en cuenta los aspectos mencionados se diseño la cara superior (véase figura 3.6) de la pinza. Figura 3.6: Cara superior de la pinza 3.3.3. Componente izquierdo y derecho compuesto por dos dedos cada uno El componente tanto izquierdo como derecho serán relativamente iguales, la diferencia radica en que el movimiento generado en el componente derecho esta determinado por el motor, y el movimiento en el izquierdo esta determinado por el derecho. Los piñones que se utilizo para el cálculo del área de las caras inferior y superior, serán ubicados en cada uno de los dedos ambos piñones están sujetos mediante pernos a la estructura de cada componente. En la estructura del componente se ha hecho dos agujeros por los cuales pasará tanto los ejes de rotación como los componentes que servirán de guía para los cuales se dejo los agujeros en la cara inferior y superior. En estos dispositivos al igual que en la cara superior se precisaba de soportes por las elevaciones que se presentan en ambos extremos de los componentes y los sitios donde van a ir ubicados los sensores. 43 3.3 Diseño de la pinza Se ha diseñado una estructura para cada dedo en forma de trapezoide, con el fin de proporcionar un mejor agarre a componentes redondos o de dimensiones grandes, mejorar la fuerza que la pinza pueda aplicar sobre los objetos a sostener, y aplicar mayores puntos de presión en determinados objetos tratando de que esto no influya en el control de la pinza. Existen agujeros hechos también, para ubicar los sensores de fuerza, la dimensión de estos corresponde a las medidas de los sensores a utilizar, es conveniente dejar un offset de 1 mm por lado a las dimensiones del sensor, esto se realiza debido a que en el momento de la impresión el material se expande y provoca que disminuya el ancho real interno y externo de la pieza. Una de las características que posee la mano humana es que la piel tiene un coeficiente de rozamiento óptimo para cualquier material lo que disminuye la fuerza que se deba aplicar sobre el mismo para sostenerlo, en este mismo contexto se ha buscado un material que sea económico y preste las mejores características con vidrio, papel o plástico, siendo el caucho uno de los mejores materiales para este trabajo, por lo tanto se dejarán las ondulaciones necesarias en la mano para colocar este material. Con los aspectos analizados se ha diseñado los componentes izquierdo y derecho con sus respectivos dedos, ondulaciones para el material anticorrosivo y los sensores de fuerza (véase figura 3.7) Figura 3.7: Componentes Izquierdo o derecho con sus respectivas partes 44 3.3 Diseño de la pinza 3.3.4. Ejes de rotación Los ejes de rotación diseñados para el presente proyecto tienen tres diámetros diferentes el primero para ajustarse al agujero que se dejo en el componente de los dedos, el segundo para servir de tope mecánico entre la cara inferior y el eje que corresponde al tercer diámetro. En la parte superior del eje que se mencionó en el párrafo anterior se ha realizado un agujero en el centro del objeto con el propósito de que en el componente izquierdo se introduzca una unión cuya función sea llegar al punto de rotación en la cara superior, y en el componente derecho sirva únicamente para fortalecer la unión entre el piñón y el componente derecho. Las dimensiones de cada estructura pueden ser determinadas de acuerdo a los requerimientos del usuario o por factibilidad en su montaje, en este caso se ha utilizado las dimensiones que se muestran en el Anexo A, el presente proyecto ha utilizado un piñón de 6 mm de diámetro interno por lo tanto el diámetro de la unión dos será de 5 mm (1 mm se espera en expansión del material de impresión ). Por lo tanto el resultado del diseño de las uniones será el que se muestra en las figuras 3.8 y 3.9 Figura 3.8: Unión entre los dedos y los ejes de la cara inferior Figura 3.9: Unión entre los dedos y los ejes de la cara superior 45 3.4 Sensores de Fuerza 3.4. Sensores de Fuerza Para la calibración y adecuación de los sensores de fuerza, se ha utilizado diferentes masas las cuales representan las variaciones de fuerza que se pudiesen aplicar al sensor; el peso de las masas varía entre 10 g hasta 1000 g, se ajustara el resultado de las mismas con el fin de obtener a la salida una variación entre 0 y 3.3 Voltios, el mismo que permitirá la lectura de datos por parte de la tarjeta de programación. Los sensores de Fuerza a utilizar son resistivos (véase figura 3.10), tienen una sensibilidad de hasta 1 kilogramo y su resistencia varía de entre 0 hasta 10 KΩ. Figura 3.10: Sensor de Fuerza Ya que su resistencia varía de entre 0 a 10 KΩse puede utilizar un divisor de voltaje, de esta manera se determinaría la resistencia teniendo como salida el voltaje máximo soportado por el Pin de entrada de la tarjeta de adquisición de datos, también con el objetivo de que haya la menor perdida de datos posibles se ha añadido un amplificador operacional que servirá como acople de impedancias, para tener la mayor transparencia posible en la transmisión de datos. Figura 3.11: Divisor de Voltaje [35] 46 3.4 Sensores de Fuerza Para determinar la resistencia Rmdel circuito que se muestra en la figura 3.11, se utiliza como referencia el esquema emitido por el fabricante, cuyos valores de resistencia vienen determinados por la ecuación 3.8. Vout =R2 1 + RF SR Rm (3.8) En la fase de experimentación se pudo corroborar el mejor comportamiento del FRSR, siendo el mismo con una resistencia con un valor igual a 2 KΩ, este valor proporciona una salida de hasta 2.8 V con carga máxima. También se tomo en consideración los diferentes puntos donde se haría presión dando como resultado que: si se genera presión en las esquinas el voltaje es diferente al que se genera en el centro del componente con una relación de 50 por ciento, es decir que la sensibilidad del mismo es mayor a medida que el punto de presión se aproxime al centro del sensor, en la figura 3.12 se puede apreciar tanto las diferentes masas que se uso para la calibración del sensor así como también el punto de presión que se utilizo de referencia para todas las medidas. Figura 3.12: Conjunto de pesas para pruebas de sensor de fuerza De igual manera se ha analizado el comportamiento del componente en diferentes tiempos de haber sido colocada la carga esto es relativamente importante al momento de realizar el estudio de los datos que emite este componente, este tipo de experimento no presento variaciones en la salida de voltaje del amplificador operacional. 47 3.4 Sensores de Fuerza Figura 3.13: Salida - Peso aplicado Resistor 10 kΩ Como se puede apreciar en la figura 3.13 el voltaje máximo de salida nos da un valor de 2 V ya que la entrada de el ADC a utilizar, permite una entrada de hasta 3.3 V se estaría perdiendo resolución por lo tanto este valor no es adecuado. Figura 3.14: Salida - Peso aplicado Resistor 1 kΩ Como se puede apreciar en las diferentes figuras el mas estable es el resistor de 2 KΩ, con el resistor de 1 KΩse desestabilizaba el sistema frecuentemente y su salida es menos constante en referencia al resistor de 2 KΩ, se ha utilizado otros valores de resistencia como 3 KΩ, 4.7 KΩ, etc, que son valores comerciales pero la resolución de la salida en determinados casos sobrepasaba los 3.3 V y en otros casos 48 3.5 Diseño de circuitos electrónicos Figura 3.15: Salida - Peso aplicado Resistor 2 kΩ tenía el mismo comportamiento que con el resistor de 1 KΩ, por lo tanto se trabajará con un Rmde 2 KΩ. El amplificador que se utilizó para la obtención de estos datos es el LM358P especificado por el fabricante [35] como uno de los más indicados para trabajar con las FRSR. 3.5. Diseño de circuitos electrónicos Una vez desarrolladas las pruebas con diferentes sensores de fuerza se procedió al diseño de las diferentes placas; la placa para los sensores de fuerza se diseñó con el objetivo de que sea lo más compacta posible, para que pueda ser contenida dentro de la pinza, en la figura 3.16 se puede apreciar el diseño final. Figura 3.16: Diseño de placa con entradas y salidas de sensores de fuerza 49 3.6 Software 3. Se almacena un nuevo dato de entrada muestras[i] = ADC() 4. Se almacena en el total la nueva muestra sumada la muestra anterior Total = Total + muestras [i] 5. Se incrementa el número de muestras obtenidas en uno i++ 6. Se repite el bucle Este proceso se lo realiza la cantidad de veces que han sido definidas para la muestra y posteriormente se promedia el Total, la eficiencia de este filtro depende del número de muestras que se adquieran del sensor. En este caso se han definido dos valores máximos para el numero de muestras siendo el primero 30 y en el segundo caso 50. Para el primer caso se ha obtenido la figura 3.24 Figura 3.24: Filtro promediador con 30 muestras En esta figura podemos observar que a pesar de que el filtrado a resultado ser eficiente aún siguen existiendo varios picos de ruido que pueden ocasionar variaciones en la etapa de control por lo tanto se procede a aumentar el número de muestras al filtro promediador a 50. Se debe tener en cuenta que se puede aumentar el numero de muestras tanto como el procesador que se este usando lo permita y dependiendo del tiempo de muestreo que se le de al sistema. 56 3.6 Software En este caso en particular el tiempo de muestreo es de 1 ms, pero cada dato será analizado en ns ya que esa velocidad de adquisición nos permite la tarjeta Discovery. Figura 3.25: Filtro promediador con 50 muestras Aumentando el número de muestras se puede evidenciar que se ha disminuido en gran proporción el ruido, no se ha aumentando el número de muestras porque no hay cambios representativos en la salida y se carga innecesariamente el procesador de la tarjeta Discovery, por lo tanto para deducir el ruido en el modelo de experimentación se trabajará con un filtro promediador con 50 muestras. 3.6.2.2. Obtener datos Para obtener los datos en primer lugar se ha definido un controlador P tal que haga subamortiguado el sistema pero sin que la acción de control sature el mismo o el sistema se vuelva inestable, luego de hacer varios experimentos se ha determinado que el valor de 3 para la ganancia en el proporcional cumple estas especificaciones. En este punto el sistema se vuelve subamortiguado en el régimen transitorio y se estabiliza en aproximadamente de 1 a 2 segundos para el régimen permanente(véase 3.26). Los diferentes escalones simbolizan la posición del motor la cual va de 0 a 180 grados, como se cuenta con un potenciómetro para definir la ubicación el mismo alimentado con 3 V dará una variación de 0 a 10 kΩ, ya que se trabaja con 12 Bits en el ADC se obtendrá 4096 valores así que la definición de posición corresponderá al rango de 0 a 4096 sin embargo mediante la practica se constata que los valores van de 57 3.6 Software 100 a 4096. Ya que existe repetibilidad en esta medida se ha agregado un offset el cual arregle este desfase quedando como resultado la figura 3.26 donde se muestra los valores obtenidos y la referencia ingresada. Figura 3.26: Sistema de experimentación con ganancia 3 en el proporcional Las posiciones que se han ingresado corresponden a 45, 90 y 135 grados respectivamente. Aunque en principio parecería alcanzar el sistema la referencia sin problemas esto no es real pero la variación es demasiado pequeña como para ser notada en la escala en la cual se presenta la figura. 3.6.2.3. Calculo de la función de transferencia Una vez Obtenidos los datos se procederá ha obtener la función de transferencia del sistema mediante la ecuación 3.9. GΘbc (s) = k∗w2 n s2+ 2 ∗ξ∗wn∗s+w2 n (3.9) Donde: kEs la ganancia wnEs la frecuencia natural ξEs el coeficiente de amortiguamiento 58 3.6 Software En primer lugar se determinará ξmediante la ecuación 3.10 ξ=−lnδ p(lnδ)2+π2(3.10) El valor de sobre-oscilación δesta determinado por 3.11 δ=Mp−Θbc(∞) Θbc(∞)(3.11) La frecuencia natural esta determinada por 3.12 wn=wp p1−ξ2(3.12) Donde wpes la frecuencia propia determinada por el tiempo de pico tp3.13 tp=π wp (3.13) Primer escalón Θbc(∞) = AK Θbc(∞) = 1083 A= 1084 K= 0,99 Mpcorresponde al valor pico y Θbc(∞)corresponde al valor alcanzado por el sistema, sustituyendo en 3.11 δ=1209 −1083 1083 δ= 0,1163 Sustituyendo en 3.10 ξ=−ln(0,1163) p(ln(0,1163))2+π2 ξ= 0,5649 Despejando y sustituyendo en 3.13 wp=π tp wp=3,14 0,767 wp= 4,08 59 3.6 Software Sustituyendo en 3.12 wn=4,08 p1−(0,5649)2 wn= 4,945 Segundo escalón Θbc(∞) = AK Θbc(∞) = 1058 A= 1084 K= 0,976 Sustituyendo en 3.11 δ=1188 −1058 1058 δ= 0,1228 Sustituyendo en 3.10 ξ=−ln(0,1228) p(ln(0,1228))2+π2 ξ= 0,555 Despejando y sustituyendo en 3.13 wp=π tp wp=3,14 0,75 wp= 4,1888 Sustituyendo en 3.12 wn=4,1888 p1−(0,555)2 wn= 5,0352 Tercer escalón Θbc(∞) = AK Θbc(∞) = 1096 60 3.6 Software A= 1084 K= 1,01 Sustituyendo en 3.11 δ=1204 −1096 1096 δ= 0,0985 Sustituyendo en 3.10 ξ=−ln(0,0985) p(ln(0,0985))2+π2 ξ= 0,5936 Despejando y sustituyendo en 3.13 wp=π tp wp=3,14 0,77 wp= 4,08 Sustituyendo en 3.12 wn=4,08 p1−(0,5936)2 wn= 5,07 Mediante el promedio de los diferentes valores obtenidos, se tiene que: k= 0,992 wn= 5,0167 ξ= 0,5711 Sustituyendo en 3.9 GΘbc (s) = 24,8987 s2+ 5,7281s+ 25,15 3.6.2.4. Simulación y validación de las ecuaciones obtenidas Para verificar que esta ecuación ha sido determinada correctamente se ha utilizado el software matlab y mediante simulink se ha ingresado la ecuación de transferencia con los diferentes escalones que se 61 3.6 Software determinaron en un principio y en un tiempo de muestreo igual al utilizado para tomar los datos, el esquema de simulink se muestra en la figura 3.27 Figura 3.27: Diseño en simulink para el esquema en bucle cerrado Los datos de esta simulación han sido comparados con el sistema real, dando como resultado la figura 3.28 Figura 3.28: Esquema real y esquema simulado con la función de transferencia obtenida En esta figura podemos observar que tanto la señal obtenida como la simulada son muy parecidas por lo cual se puede definir que esta correctamente implementada la función de transferencia. Esta ecuación se ha deducido en bucle cerrado, para determinar la ecuación en bucle abierto se utilizará 3.14 GΘ=GΘbc (s) Kp(1 −GΘbc (s)) (3.14) Dando como resultado GΘ=8,3 s2+ 5,728s+ 0,2513 62 3.6 Software Se procede a ingresar los resultados en el simulador y se comparará con la señal en bucle cerrado teniendo que ser exactamente iguales las dos dando a la señal en bucle abierto un kp de 3 que es valor que se ha dado a la ganancia del proporcional para deducir el sistema, quedando el esquema de simulación como se muestra en la figura 3.29 Figura 3.29: Esquema en bucle abierto y en bucle cerrado Este sistema implementado nos da como resultado Figura 3.30: Comparación en bucle abierto y en bucle cerrado Como se puede observar en la figura 3.29 ambos sistemas se corresponden por lo tanto la deducción de bucle abierto está bien desarrollada. 3.6.2.5. Diseño de controlador Con el objetivo de eliminar el amortiguamiento y mejorar el tiempo de establecimiento que existe en el sistema se ha utilizado la herramienta sisotool de matlab, mediante la cual se ha implementado un cero para mejorar la respuesta en el transitorio y su ganancia para eliminar el amotiguamiento que existe en 63 3.6 Software el sistema. La configuración que se ha realizado en sisotool se muestra en la figura 3.31 Figura 3.31: Diseño del controlador PD Sisotool En esta figura también se puede apreciar que el cero está ubicado dentro de especificaciones así como también los polos. La respuesta al impulso que da esta configuración se muestra en la figura 3.32, esta misma respuesta se ha utilizado para adaptar el sistema variando la ubicación de los polos, los ceros y la ganancia. Figura 3.32: Respuesta al impulso mediante Sisotool Una vez diseñado este sistema se ha realizado los cálculos pertinentes para el sistema, utilizando la ecuación 3.15 64 3.6 Software GRPID(s) = kc(Tds2+s+1 Ti) s(3.15) Donde por deducción y para un PD se determina que Td= 1/b (3.16) kc=kp(b)(3.17) El valor de kpcorresponde a la ganancia que en este caso es de 53.312 y el valor de b es el cero que se ha agregado que en este caso corresponde a 24.2. Por lo tanto mediante 3.16 se obtiene el tiempo para el derivativo el cual es igual a: Td= 0,04132 Y el valor de la ganancia mediante 3.17 para el derivativo será igual a: kc= 1290,15 Se ha implementado estos valores en el entorno de simulación simulink obteniendo los resultados que se muestran en la figura 3.33 Figura 3.33: Respuestas ante referencia con controlador PD y P Como se puede apreciar el sistema deja de estar sobreamortiguado, mejora su tiempo de estabilización y alcanza la referencia en el régimen transitorio y en el permanente. 65 3.6 Software Figura 3.40: Esquema de funcionamiento para Posición Se leerá la posición actual del servo mediante la línea de código. readPos();. La función de este comando es leer el PIN análogo del GPIO correspondiente al potenciómetro (PE6), el valor generado por esta función será almacenado en una variable global de modo que se pueda ocupar en otras funciones. Este valor conjuntamente con el de referencia al cual se desea llegar serán trasmitidos para que proporcionen la acción de control mediante el comando. PIDControl(valPos, valSensor); Controlador.- El software que se ha implementado para el control del motor es un PD en paralelo cuyos cálculos se realizaron en la sección de identificación. Se ha desarrollado a modo de prueba también un controlador PID sin embargo existía acumulación del error en el integral, lo cual volvía inestable el sistema, generando sobreoscilaciones al momento de llegar a la referencia, para tal efecto se siguió el esquema indicado en la figura 3.41, para el PD se elimina 72 3.6 Software la parte integral del controlador en paralelo la ganancia y el cero agregado para cumplir especificaciones se han analizado en la etapa de identificación. Figura 3.41: Esquema de control PID[38] En la actualidad muchos dispositivos de carácter industrial utilizan controladores PID o derivados de los mismos, pudiendo ser estos de tipo mecánico, neumático, electrónico, etc, mediante el uso de microprocesadores se puede implementar este tipo de controladores de forma digital, ya que en un controlador PID intervienen tres parámetros diferentes como lo son; de tipo proporcional, integral y derivativo, respondiendo a la ecuación 3.21. u(t) = kp∗e(t) + kp Ti ∗Zt 0 e(t)∂(t) + kp∗Td∗∂(e(t)) ∂(t)(3.21) Para la lectura de sensores se ha establecido un tiempo de muestreo de 1 ms, este tiempo de muestreo se ha determinado mediante un timer el cual activará cada mili-segundo una interrupción la cual llamará a la función que realizará la lectura de los datos analógicos pudiendo ser estos datos por parte de los sensores de presión o por parte del potenció metro. Mediante la siguiente línea de código se crea el timer en el cual se establece el tiempo de activación de la interrupción denominada CustomTimer1_Task. CustomTimer1 = TM_DELAY_TimerCreate ( 1, 1, 1, CustomTIMER1_Task, NULL); En cada instante que se toman las muestras serán almacenadas en una variable global para ser usadas en la etapa de control. 73 3.6 Software Para aplicar el controlador se enviará dos parámetros tanto la referencia como la lectura del sensor, los mismos se enviarán a la siguiente función. PIDControl(valRef, valSensor); Dentro de esta función se ha implementado el controlador PD si el caso se diese de que se necesitare un controlador PID su algoritmo de control es el siguiente: Figura 3.42: Algoritmo de control PID Ya que es un controlador en paralelo tipo PD únicamente se elimina la parte integral del mismo. Este algoritmo nos dará como resultado la acción de control que modificará el duty del PWM para de esta forma llegar a la posición indicada. Para el ajuste de las constantes del controlador se han desarrollado varias pruebas presentando en las que no se han ajustado al sistema sobre-oscilaciones ya sea en el punto de mayor apertura de la pinza u en otros puntos de posición, inestabilidad, sobre esfuerzos, movimientos bruscos, etc, son algunos de los problemas que durante el ajuste de las constantes del PD se pudieron apreciar, hasta llegar a los valores que no producían ninguno de estos efectos. El posicionamiento o apertura de la pinza se lo realizara en grados pudiendo ser la apertura máxima de 115 y su cierre máximo de 145 grados. 74 3.6 Software Basando las pruebas del controlador implementado se han realizado diferentes pruebas en este rango dando como resultado el desplazamiento esperado tal como se muestra en la figura 3.43 Figura 3.43: Apertura de la pinza dentro del rango que va de 115 a 145 grados Se puede apreciar como el sistema llega a la posición esperada sin sobre-oscilaciones en el régimen transitorio y además cumple con la referencia, el análisis se baso en tres escalones de 123, 130 y 135 grados correspondientes a cada escalón, quedando verificado la eficiencia del controlador PD diseñado. 3.6.3.3. Control por Fuerza Para el control por fuerza se ha seguido el mismo procedimiento que el implementado para control por posición la diferencia entre estos dos procesos serán los datos que serán utilizados para realizar la acción de control. En este caso los datos provienen de la lectura de los sensores de presión ubicados tanto en las puntas de los dedos como en los bordes internos de cada uno de ellos. Es importante tener en consideración que para desplazarse el motor necesita al menos el 40 %de su capacidad total por lo tanto en tramos pequeños la acción de control que se pueda generar necesita la ganancia suficiente para lograr este movimiento. Es por este motivo que a partir de una distancia determinada se variarán las constantes del controlador con el fin de lograr desplazarse en tramos pequeños para que el motor pueda producir mayor fuerza al momento de presionar. 75 3.6 Software Se han variado las constante del controlador de modo que se pueda producir este efecto, sin embargo los datos no se han podido mostrar debido a que el uso de la SDRAM ocupa GPIO’s estimados para la lectura del ADC. Para la aplicación del esquema de fuerza se analizará la estructura de funcionamiento a ejecutar (véase figura 3.44) Figura 3.44: Esquema de funcionamiento para Fuerza Cada uno de estos sensores transmitirán información la cual puede ser analizada en conjunto o independientemente dependiendo de la acción que se desea realizar. Tal es el caso de que si se desea tomar una botella dependiendo del tamaño se pudiesen utilizar los sensores ubicados en las puntas o los internos, pero si el caso fuese tomar un bolígrafo u objetos más pequeños se podría utilizar únicamente los sensores de las puntas. El análisis de estos sensores se podría hacer de varias formas y dependiendo de las conclusiones u acciones a las cuales queramos llegar, para el presente proyecto se ha optado por realizar la captura de 76 3.6 Software datos de todos los sensores y sacar el promedio de todos ellos de esta manera se estimara la fuerza que se esta aplicando, y comparandola con la referencia mediante el controlador se determinará la acción de control. El esquema de control para Fuerza se muestra en la figura 3.45 Figura 3.45: Esquema de control para Fuerza El selector que se muestra en el esquema realizará la selección de cuantos y cuales dedos se va a utilizar para el análisis de fuerza aplicada, por lo tanto la llamada a la interrupción se hará de la misma forma que en el caso de posición con la diferencia que contendrá el parámetro finguers que determina que dedos se va a utilizar, de la siguiente manera. CustomTimer1 = TM_DELAY_TimerCreate ( 1, 1, 1, CustomTIMER1_Task, finguers); Aplicando esta línea de código se lograra realizar la lectura de los ADC necesarios. El resultado de la lectura de los sensores y la fuerza de referencia serán transmitidos al controlador, donde también han sido ajustados los parámetros de las constantes para no producir sobre-oscilaciones, movimientos bruscos, o golpes a los objetos que se puedan producir por la inercia con la que se mueve el motor. Para tener constancia de la fuerza que esta aplicando la pinza sobre el objeto se han realizado diferentes experimentos mediante los cuales; con poca fuerza no pueda levantar o sujetar determinados objetos, y con el incremento de la misma se los pueda sujetar y manipular sin problemas(vease figuras 3.46, 3.47, 3.48) 77 3.6 Software Figura 3.46: Objeto sujetado con una fuerza equivalente al 80 %de la capacidad total de la pinza 78 3.6 Software Figura 3.47: Objeto sujetado con una fuerza equivalente al 90 %de la capacidad total de la pinza 79 3.6 Software Figura 3.48: Objeto sujetado con una fuerza equivalente al 50 %de la capacidad total de la pinza 80 FUNCIONAMIENTO Y PRUEBAS 5 CONCLUSIONES Y RECOMENDACIONES Cumplidos finalmente los objetivos propuestos y finalizada la investigación y ejecución del presente TFM, se exponen las siguientes conclusiones y recomendaciones. 5.1. Conclusiones 1. Queda demostrado mediante la práctica y experimentación, que es posible la implementación y control de un pinza con un único servo-motor de bajas capacidades. 2. Se ha diseñado una pinza la cual cuenta con cuatro dedos funcionales los cuales mediante la practica han determinado su funcionamiento, mejorando el agarre, para determinados objetos. 3. La calibración de los sensores de fuerza es básica ya que de esta depende el control que posteriormente tendrá la pinza en el sentido de la fuerza con la que ejecute las acciones de agarre. 4. El potenciómetro con el cual cuenta el servo motor genera demasiado ruido a la salida del mismo, mediante la experimentación y con un número determinado en 50 muestras se ha podido disminuir eficientemente la cantidad de ruido que este genera. 5. La identificación de la función de transferencia que se ha realizado para el presente proyecto a permitido desarrollar controladores que funcionen de manera optima tanto para posición como para fuerza 6. La comunicación USB mediante la experimentación se ha definido que no es lo suficientemente robusta como para trasmitir datos cada mili segundo produciéndose: perdida de datos, colapso en el sistema, lectura de datos errónea por parte de la tarjeta al ser sobrecargado el procesador de la misma. 7. El rango máximo de movilidad de la pinza esta determinado a 30 grados, dependiendo del diseño este se pudiese ampliar o reducir. 8. El uso de la SDRAM es necesario para poder almacenar los datos que posteriormente serán presentados por el canal USB, para lo cual se ha definido una dirección de memoria de 16 bits por 88 5.1 Conclusiones muestra, se ha llegado a almacenar en el presente proyecto hasta 50000 muestras en la fase de experimentación e identificación sin presentar error alguno 9. Si los datos presentados por la SDRAM son totalmente incoherentes al análisis que se esta realizando, es probable que no se hayan almacenado los datos o no este leyendo correctamente la dirección donde se ha almacenado los mismos. 10. El motor para iniciar su movimiento necesita al menos del 40 %de su capacidad total, esto se debe al rozamiento que existe entre los ejes a los cuales se encuentra sujeto el motor y los diferentes componentes que acompañan al movimiento, como los dedos, sensores, piñones, etc. 11. El controlador que se ha desarrollado para el control de posición cumple especificaciones, evitando el sobre-amortiguamiento del sistema en el régimen transitorio y la estabilidad del mismo en el régimen permanente, también alcanza sin problemas la referencia. 12. Se ha probado el motor para diferentes posiciones y no ha dado problemas para alcanzar las mismas salvo en aquellos casos que se quiera trasladar de un grado a su inmediato superior en la escala de uno debido al rozamiento, ya que este parámetro ha sido ajustado mediante la practica el sistema ejecuta el movimiento para la mayoría de los casos. 13. La constante que se ha determinado para medir la velocidad se lo ha hecho llevando el motor desde su posición mas baja a la mas alta y evaluando la pendiente que este tipo de movimientos ha producido, constatando de que el valor generado es constante y uniforme, las pequeñas variaciones que en el mismo se presentan se deben al ruido del potenciometro. 14. Se ha probado la estimación de fuerza con diferentes objetos siendo necesario aplicar mayor fuerza a los que tienen mayor peso para poder sostenerlos, a los mismos si se les aplica menor fuerza la pinza no es capaz de sostenerlos, validando el funcionamiento de este tipo de control. 15. No ha sido posible constatar visualmente los datos proporcionados por cada uno de los sensores de fuerza ya que al momento de utilizar los mismos, no se puede trabajar con la sdram, lo cual evita que se guarden los datos generados por los sensores e imposibilita la lectura de los mismos a través de la pantalla, sin embargo para determinar que los sensores están llegando a ejercer la fuerza indicada, se ha hecho uso de un condicional el cual devolvería un determinado estado cuando la fuerza aplicada sea igual a la referencia, obteniendo resultados positivos. 89 5.2 Recomendaciones 5.2. Recomendaciones 1. Es recomendable trabajar con un motor de mejores prestaciones que el usado en el presente proyecto de esta manera se podría aplicar mayor presión a los objetos. 2. Tener un buen diseño de la pinza es un punto indispensable en la partida del proyecto, en el cual se deben considerar las acciones que se va a realizar, el peso que tiene que soportar, la fuerza que va a ejercer, etc. 3. No sobre-esforzar el modelo de prueba si el mismo esta desarrollado en ABS, este material es demasiado frágil para aplicar cantidades altas de torque en el motor pudiendo producir fisuras o la in-habilitación del modelo experimental. 4. Se debe tener especial consideración en el manejo y programación de la interfaz USB ya que esta puede servir de gran ayuda en la obtención de datos e interacciones repetitivas con el dispositivo pudiendo variar los datos repetidamente a modo de experimentación. 5. Es necesario tener una idea clara sobre lo que se va ha hacer y que dispositivos se va a usar si los dispositivos que se van a usar son los mencionados en el presente proyecto es recomendable empezar por montar el USB, SDRAM, ADC, PWM, GPIO, en ese orden respectivamente, esto evitará que se tenga que modificar código cada vez que se adhiere un nuevo dispositivo. 6. Es importante tomar varias muestras para poder definir un modelado eficiente para la pinza diseñada, sobre todo es recomendable experimentar en le rango que se va a trabajar y con todos los dispositivos montados, así no se tendrá que modificar en lo posterior constantes como las ganancias del controlador o la constante de rozamiento. 7. Al momento de compilar el programa es preferible tener deshabilitada la opción de busqueda dentro del Target esto aumentará la velocidad con la cuál el software compila. 90 Bibliografía [1] Sánchez Martín FM, Millán Rodríguez F, Salvador Bayarri J, Palou Redorta J, Rodríguez Escovar F, Esquena Fernández S, Villavicencio Mavrich H Historia de la robótica: de Arquitas de Tarento al robot Da Vinci, pp. 69-75 ,(2006) 6, 7 [2] RIA Robotic Industries Association 12 Diciembre. (2015),[en línea],disponible en:http://www. robotics.org/ 7 [3] Alarcon, Maria Isabel Fidelity: el futuro de la automatización robotizada 12 Diciembre. (año),[en línea],disponible en:http://www.rankia.com/blog/fondos-inversion/ 2417082-fidelity-futuro-automatizacion-robotizada iv, 7 [4] Pérez, Omar Agricultura 15 Diciembre. (2015),[en línea],disponible en:http://perezrodriguez1. blogspot.com.es/2014/07/robotica-en-la-agricultura-y-la.html 8 [5] Universidad Carlos III Madrid Robots que reducen tiempo y dinero en la construcción 15 Diciembre. (2015),[en línea],disponible en:http://portal.uc3m.es/portal/page/portal/ actualidad_cientifica/noticias/robots_construccion 9 [6] Valparaiso Robótica Submarina 17 Diciembre. (2015),[en línea],disponible en:http://www. skyringmarine.cl/index.htm 9 [7] RT Video: marines de EE.UU practican con el asombroso robot militar de Google 18 Diciembre. (2015),[en línea],disponible en:https://actualidad.rt.com/actualidad/ 186615-video-marines-eeuu-practican-asombroso 9 [8] Dr. Gene Emmer Exoesqueleto 05 Enero. (2016),[en línea],disponible en:http://www. exoesqueleto.com.es/iquestcoacutemo-funciona-un-exoesqueleto.html 14 [9] Samuel Franco Domínguez Blog de rehabilitación que mira el futuro 17 Enero. (2016),[en línea],disponible en:http://www.rehabilitacionblog.com/2012/02/ exoesqueleto-robotico-de-kawasaki.html iv, 14 [10] Marsi-Bionics Smart Teraphy 17 Enero. (2016),[en línea],disponible en:http://www.smartherapy. org/crowdfunding-marsi-bionics/ iv, 14 [11] Francisco Pérez Marcial Diseño, Análisis y Modelado Cinemático de un Exoesqueleto Pasivo de Extremidad Inferior con Propósito de Rehabilitación, pp. 17-100 ,(2011) 16, 19 [12] Ing. Andrés E. Gómez H., Ing. Mauricio Carrillo, MSc. John Faber Archila Díaz ESTUDIO DE AYUDAS TECNICAS TIPO EXOESQUELETO PARA APOYO A PACIENTES CON DESORDENES EN EL MOVIMIENTO, pp. 52-59 ,(2009) 17 [13] J. Rosen, M. Brand, M. Fuchs, and M. Arcan A myosignal-based powered exoskeleton system. IEEE Transaction On Systems, Man and Cybernetics - Part A: Systems and Humans, pp. 210-222 ,(2001) 17 [14] Lokheed Martin Lokheed Martin 17 Enero. (2016),[en línea],disponible en:http://www. lockheedmartin.com/ iv, 18, 19 91 BIBLIOGRAFÍA [15] Michael C. Yipa, Mahdi Tavakoliband, Robert D. Howe Performance Analysis of a Haptic Telemanipulation Taskunder Time Delay, pp. 651-673 ,(2011) iv, 20 [16] Cyber Grasp Cyber Glove System 20 Enero. (2016),[en línea],disponible en:http://www. cyberglovesystems.com/cybergrasp/ iv, 21 [17] Lokomat TERAPIA FUNCIONAL DE MARCHA ROBOTIZADA 22 Enero. (2016),[en línea],disponible en:https://www.hocoma.com/world/en/products/lokomat/ iv, 21 [18] J. A. Cozens. Robotic assistance of an active upper limb exercise in neurologically impaired patient. IEEE Transaction On Rehabilitation Engineering, pp. 254-256 ,(1999) 22 [19] H. I. Krebs, N. Hogan, M. L. Aisen, and B. T. Volpe. Robot-aided neurorehabilitation. IEEE Transaction On Rehabilitation Engineering,, pp. 75-88 ,(1998) 22 [20] Equipo de Amazings Prótesis robótica con músculos artificiales para rehabilitación de pie y tobillo 08 Febrero. (2016),[en línea],disponible en:http://noticiasdelaciencia.com/ iv, 22 [21] Christian Ortega Loubon1, Julio César Franco2. Neurofisiología del aprendizaje y la memoria. Plasticidad Neuronal, pp. 1-7 ,(2010) 23 [22] Victor R. Gonzalez Elementos terminales o actuadores finales, 10 Febrero. (2016),[en línea],disponible en:http://platea.pntic.mec.es/vgonzale vii, 25 [23] FIB Elementos Finales 15 Febrero. (2016),[en línea],disponible en:http://www-pagines.fib.upc. es 27 [24] Sauloma Pinzas y ejes de mano 18 Febrero. (2016),[en línea],disponible en:http://www. profesaulosuna.com iv, 28, 29 [25] Siegert, H.-J.; Bocionek, S.: Robotik Programmierung intelligenter Roboter, pp. 446-523 ,(1996) 28 [26] César Augusto Quinayás-Burgos, Mariela Muñoz-Añasco, Óscar Andrés Vivas-Albán, Carlos Alberto Gaviria-López Diseño y construcción de la prótesis robótica de mano, pp. 224236 ,(2010) 29 [27] Christian Augusto Silva Castellanos, Jhon Edison Muñoz Riaños, Diego Alexander Garzón Alvarado, Nancy Stella Landínez Parra, Octavio Silva Caicedo Revista Cubana de Investigaciones Biomédicas, pp. 15-41 ,(2011) 29 [28] ottobock Prótesis de mano SensorHand Speed y VariPlus Speed 01 Marzo. (2016),[en línea],disponible en:http://www.ottobock.es/ 30 [29] Kawassaki departamento de mecatrónica robótica 22 Enero. (2016),[en línea],disponible en:http: //www.rm.kanagawa-it.ac.jp/ iv,iv, 23, 24 [30] Andrés Felipe Ruíz Olaya Sistema Robótico Multimodal para Análisis y Estudios en Biomecánica, Movimiento Humano y Control Neuromotor, pp. 12-256 ,(2008) iv,iv, 10, 12, 19 [31] K.P. Tee, E. Burdet, C.M. Chew, and T.E. Milner A model of force and impedance in human arm movements, pp. 368-375 ,(2004) iv, 11 [32] G. J. Monkman, S. Hesse, R. Steinmann, H. Schunk Robot Grippers, pp. 19-72 ,(2007) iv, 33, 35 [33] UN ESPACIO PARA LA FÍSICA!!! Conociendo la física 04 Abril. (2016),[en línea],disponible en:https://conociendolafisica.files.wordpress.com/2010/04/8mcu.pdf 37 [34] RS RS Components 07 Abril. (2016),[en línea],disponible en:http://docs-europe. electrocomponents.com/webdocs/09ad/0900766b809ad3ef.pdf iv, 41 92 BIBLIOGRAFÍA [35] Interlink Electronics FSRR Integration Guide y Evaluation Parts Catalog, pp. 1-24 ,(2009) v, 46, 49 [36] Texas Instruments TL7660CMOS VOLTAJE CONVERTER, pp. 1-23 ,(2006) 50 [37] Texas Instruments L293x Quadruple Half-H Drivers, pp. 1-21 ,(2015) 50 [38] Ilber Adonayt Ruge Ruge METODO BASICO PARA IMPLEMENTAR UN CONTROLADOR DIGITAL PID EN UN MICROCONTROLADOR PIC PARA DESARROLLO DE APLICACIONES A BAJO COSTO, pp. 1-10 ,(2014) v, 73 [39] Tilen Majerle stm32f4 Discovery Mayo-julio. (2016),[en línea],disponible en:http:// stm32f4-discovery.net 53, 82 93 ANEXOS 94 ANEXO A Cotas de la pinza 95 3 5 5 3 3 R20,50 R25,50 R20,50 R25,50 70 35 35 5 49 37,33 base_engranaje_25.6mm PESO: A4 HOJA 1 DE 2 ESCALA:1:1 N.º DE DIBUJO TÍTULO: REVISIÓN NO CAMBIE LA ESCALA MATERIAL: FECHA FIRMA NOMBRE REBARBAR Y ROMPER ARISTAS VIVAS ACABADO: SI NO SE INDICA LO CONTRARIO: LAS COTAS SE EXPRESAN EN MM ACABADO SUPERFICIAL: TOLERANCIAS: LINEAL: ANGULAR: CALID. FABR. APROB. VERIF. DIBUJ. Figura A.1: Medidas Cara inferior de la pinza 96 70 56 14 10 5 4 4 4 4 37,33 R25,50 R27 R25,50 R20,50 10,50 20 5 7 40 7 5 70 49 37,33 15,90 15,90 20 7 7 10 10 5 Superior_engranaje_25.6mm PESO: A4 HOJA 1 DE 1 ESCALA:1:1 N.º DE DIBUJO TÍTULO: REVISIÓN NO CAMBIE LA ESCALA MATERIAL: FECHA FIRMA NOMBRE REBARBAR Y ROMPER ARISTAS VIVAS ACABADO: SI NO SE INDICA LO CONTRARIO: LAS COTAS SE EXPRESAN EN MM ACABADO SUPERFICIAL: TOLERANCIAS: LINEAL: ANGULAR: CALID. FABR. APROB. VERIF. DIBUJ. Figura A.2: Medidas Cara superior de la pinza 97 ANEXO D Código desarrollado D.1. main.c /** * Keil project for gripper * * * @author Jefferson Camacho * @email [email protected] * @website --------------- * @ide Keil uVision 5 * @packs STM32F4xx Keil packs version 2.2.0 or greater required * @stdperiph STM32F4xx Standard peripheral drivers version 1.4.0 or greater required */ #include "stm32f4xx.h" #include "defines.h" #include "tm_stm32f4_pwm.h" #include "tm_stm32f4_disco.h" #include "tm_stm32f4_delay.h" #include "tm_stm32f4_adc.h" #include <stdio.h> #include "accions.h" #include "tm_stm32f4_usb_vcp.h" #include "defines.h" #include <string.h> #include <stdlib.h> #include "tm_stm32f4_sdram.h" #define LED GPIO_PIN_2 char txt[10],tyt[21]; int state=1; uint8_t c,i; void setBuffer(void) { int total = strlen(txt); for (int x=0; x <= total; x++) { txt[x]= ’\0’; } 104 D.1 main.c total = strlen(tyt); for (int x=0; x <= total; x++) { tyt[x]= ’\0’; } i=0; } void options(void) { Delayms(100); TM_USB_VCP_Puts("CHOOSE ACTION TO PERFORM\n"); TM_USB_VCP_Puts("# 1 OPEN GRIPPER\n"); TM_USB_VCP_Puts("# 2 CLOSE GRIPPER\n"); TM_USB_VCP_Puts("# 3 CONTROL GRIPPER POSITION\n"); TM_USB_VCP_Puts("# 4 CONTROL GRIPPER FORCE\n"); TM_USB_VCP_Puts("# 5 CONTROL GRIPPER VELOCITY\n\n"); } void acciones(int a) { int b; switch(a) { case 1: TM_USB_VCP_Puts("# 1 OPEN GRIPPER\n"); state = 1; while(state == 1) { state = gripperPosControl(120); } setBuffer(); break; case 2: TM_USB_VCP_Puts("# 2 CLOSE GRIPPER\n"); state = 1; while(state == 1) { state = gripperPosControl(143); } setBuffer(); break; case 3: setBuffer(); state = 5; TM_USB_VCP_Puts("ENTER POSITION\n"); while(state == 5) { if (TM_USB_VCP_Getc(&c) == TM_USB_VCP_DATA_OK) { TM_USB_VCP_Putc(c); if (c == 13) { a = atoi(txt); if (a <= 109 || a >= 146) { TM_USB_VCP_Puts("THE POSITION IS NOT POSSIBLE RANGE 115 TO 143\n"); TM_USB_VCP_Puts("ENTER ANOTHER VALUE\n"); 105 D.1 main.c setBuffer(); } else { state = 1; sprintf(tyt,"Movimiento a: %d\n",a); TM_USB_VCP_Puts(tyt); while(state == 1) { state = gripperPosControl(a); } setBuffer(); } } else { txt[i] = (char) c; i++; } } } break; case 4: setBuffer(); state = 5; TM_USB_VCP_Puts("ENTER FORCE\n"); while(state == 5) { if (TM_USB_VCP_Getc(&c) == TM_USB_VCP_DATA_OK) { TM_USB_VCP_Putc(c); if (c == 13) { a = atoi(txt); if (a <= 1200 || a >= 4000) { TM_USB_VCP_Puts("THE FORCE IS NOT POSSIBLE RANGE 1500 TO 3500\n"); TM_USB_VCP_Puts("ENTER ANOTHER VALUE\n"); setBuffer(); } else { setBuffer(); TM_USB_VCP_Puts("SELECT FINGERS TO USE FROM 0 TO 3\n"); while (state == 5) { if (TM_USB_VCP_Getc(&c) == TM_USB_VCP_DATA_OK) { TM_USB_VCP_Putc(c); if (c == 13) { b = a; a = atoi(txt); if (a < 0 || a > 3) { TM_USB_VCP_Puts("THE OPTION IS NOT POSSIBLE RANGE 0 TO 3\n"); TM_USB_VCP_Puts("ENTER ANOTHER VALUE\n"); setBuffer(); } 106 D.1 main.c else { state = 1; while(state == 1) { state = gripperForceControl(b,a); } setBuffer(); } // state = gripperForceControl(2000,0); // // if (state == 2) // // TM_USB_VCP_Puts("Sharp Object\n"), state = 1; // // if (TM_USB_VCP_Getc(&c) == TM_USB_VCP_DATA_OK) // // { // // TM_USB_VCP_Putc(c); // // if (c == 13) // // { // // TM_USB_VCP_Puts("Loose Object\n"), state = 2; // // state = gripperPosControl(130); // // } // // } // } // setBuffer(); // } } else { txt[i] = (char) c; i++; } } } } } else { txt[i] = (char) c; i++; } } } break; case 5: break; } Delayms(100); } int main(void) { //int16_t read; int a; // // /* Initialize system */ SystemInit(); // // /* Initialize delay functions */ TM_DELAY_Init(); startPWM(); 107 D.1 main.c startADC(); TM_USB_VCP_Init(); // TM_ADC_Init(ADC1, ADC_Channel_9); // TM_ADC_Init(ADC1, ADC_Channel_11); /* Initialize onboard leds */ TM_DISCO_LedInit(); //startOpenCloseControl(); // /* Set PWM to 1KHz frequency on timer TIM2 */ // /* 50Hz = 20ms = 20000us */ // TM_PWM_InitTimer(TIM1, &TIM1_Data, 1000); // TM_PWM_InitTimer(TIM2, &TIM2_Data, 1000); // // /* Initialize PWM on TIM2, Channel 1 and PinsPack 2 = PA5 Conectado al Pin 2 del Puente H*/ // /* Initialize PWM on TIM1, Channel 2 and PinsPack 1 = PA7 Conectado al Pin 7 del Puente H*/ /* BOTONES de fin de carrera conectados a PD2 y PD3 verificaciÛn en estado alto*/ /* Valores ADC Para control del Motor Conectados al PIN PF6*/ // // /* Set channel 1 value, 1500us = servo at center position */ // //TM_PWM_SetChannelMicros(&TIM2_Data, TM_PWM_Channel_1, 00); // /*Puertos de ConexiÛn ADC ADC_Channe0 0 PA0 ADC_Channe3 0 PA3 ADC_Channe7 0 PA7 ADC_Channe8 0 PB0 ADC_Channe9 0 PB1 ADC_Channel0 0 PC0 ADC_Channel1 0 PC1 ADC_Channel2 0 PC2*/ // TM_GPIO_Init(GPIOD, LED, TM_GPIO_Mode_OUT, TM_GPIO_OType_PP, TM_GPIO_PuPd_NOPULL, TM_GPIO_Speed_High); if (TM_SDRAM_Init()) { TM_DISCO_LedOn(LED_GREEN); } else { TM_DISCO_LedOn(LED_RED); } while (1) { if (TM_USB_VCP_GetStatus() == TM_USB_VCP_CONNECTED) { TM_DISCO_LedOn(LED_RED); /* If something arrived at VCP */ if (TM_USB_VCP_Getc(&c) == TM_USB_VCP_DATA_OK) { TM_USB_VCP_Putc(c); //Acciones(c); if (c == 65 || c == 97) //if c = a or A { options(); setBuffer(); } else if (c == 13) //key Carrer { a = atoi(txt); //Take all data of txt 108 D.1 main.c if (a <= 0 || a >= 6) { TM_USB_VCP_Puts("OPTION NOT AVAILABLE\n\n"); setBuffer(); //Clean Buffer } else { acciones(a); //Make the action of a TM_USB_VCP_Puts("Command executed\n\n"); setBuffer(); } } else { txt[i] = (char) c; //Convert char i++; } } } ////%%%%%%%%%%%%%%%%%%%%%%%%%%%%EVALUACION POSICION%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //if (TM_USB_VCP_GetStatus() == TM_USB_VCP_CONNECTED) // { // /* Turn on GREEN led */ // TM_DISCO_LedOn(LED_RED); // /* If something arrived at VCP */ // if (TM_USB_VCP_Getc(&c) == TM_USB_VCP_DATA_OK) // { // /* Return data back */ // TM_USB_VCP_Putc(c); // if (c == 13) // { // state = 1; // while (state == 1) // { // TM_GPIO_SetPinValue(GPIOD, LED, 1); // state = gripperPosControl(125); // aplicar mÌnimo 1500 // for(int i = 0; i < 5000; i++) // { // read = TM_SDRAM_Read16(x); // Delayms(25); // sprintf(tyt,"%d\n",read); // TM_USB_VCP_Puts(tyt); // x = x+16; // } // x = 0; // state = gripperPosControl(135); // aplicar mÌnimo 1500 // for(int i = 0; i < 5000; i++) // { // read = TM_SDRAM_Read16(x); // Delayms(25); // sprintf(tyt,"%d\n",read); // TM_USB_VCP_Puts(tyt); // x = x+16; // } // x = 0; // state = gripperPosControl(143); // aplicar mÌnimo 1500 // for(int i = 0; i < 5000; i++) // { 109 D.1 main.c // read = TM_SDRAM_Read16(x); // Delayms(50); // sprintf(tyt,"%d\n",read); // TM_USB_VCP_Puts(tyt); // x = x+16; // } // x = 0; // } // } // } // } ////%%%%%%%%%%%%%%%%%%%%%%%%%%EVALUACION POSICION%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%EVALUACION VELOCIDAD%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //if (TM_USB_VCP_GetStatus() == TM_USB_VCP_CONNECTED) // { // /* Turn on GREEN led */ // TM_DISCO_LedOn(LED_RED); // /* If something arrived at VCP */ // if (TM_USB_VCP_Getc(&c) == TM_USB_VCP_DATA_OK) // { // /* Return data back */ // TM_USB_VCP_Putc(c); // if (c == 13) // { // state = 1; // while (state == 1) // { // TM_GPIO_SetPinValue(GPIOD, LED, 1); // state = openGripper(500,143); // aplicar mÌnimo 1500 // for(int i = 0; i < 5000; i++) // { // read = TM_SDRAM_Read16(x); // Delayms(25); // sprintf(tyt,"%d\n",read); // TM_USB_VCP_Puts(tyt); // x = x+16; // } // x = 0; // } // } // } // } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%EVALUACION VELOCIDAD%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%EVALUACION ADC’s%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // int result; // int result1; // gripperForceControl(2000,0); // result = TM_ADC_Read(ADC1, TM_ADC_Channel_9); // result1 = TM_ADC_Read(ADC1, TM_ADC_Channel_11); // //result = readADC(0); // if (result > 1500 || result1 > 1500) // { // TM_GPIO_SetPinValue(GPIOD, LED, 1); // } // else // { // TM_GPIO_SetPinValue(GPIOD, LED, 0); // } 110 D.2 accions.c //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%EVALUACION ADC’s%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% } } D.2. accions.c /** * @author Jefferson Camacho * @email [email protected] * @version v1.0 * @ide Keil uVision * @license GNU GPL v5 * @brief Accions Gripper library for STM32F4xx * @verbatim ---------------------------------------------------------------------- Copyright (C) Jefferson Camacho, 2016 This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. ---------------------------------------------------------------------- @endverbatim */ #include "stm32f4xx.h" #include "defines.h" #include "tm_stm32f4_pwm.h" #include "tm_stm32f4_disco.h" #include "tm_stm32f4_delay.h" #include "tm_stm32f4_adc.h" #include <stdio.h> #include "accions.h" #include <time.h>//incluida para temporizar la funciÛn PD_trayectoria #include <string.h> #include <stdint.h>//clock //#include "tm_stm32f4_fatfs.h" #include "tm_stm32f4_sdram.h" #include "tm_stm32f4_usb_vcp.h" #define ButtonA GPIO_PIN_2 #define ButtonB GPIO_PIN_3 TM_PWM_TIM_t TIM2_Data; TM_PWM_TIM_t TIM1_Data; 111 D.2 accions.c int8_t State; int16_t valSensorPos; int16_t valSensorForce; int timeCont=0; void CustomTIMER1_Task(void* UserParameters); int PIDControl(uint16_t valRef, uint16_t valSensor, int8_t ctrl); TM_DELAY_Timer_t* CustomTimer1; //int8_t count; int cont; char tyt1[21]; void setBuffer1(void) { int total = strlen(tyt1); for (int x=0; x <= total; x++) { tyt1[x]= ’\0’; } } void startOpenCloseControl(void) { TM_GPIO_Init(GPIOD, ButtonA, TM_GPIO_Mode_IN, TM_GPIO_OType_PP, TM_GPIO_PuPd_DOWN, TM_GPIO_Speed_Low); TM_GPIO_Init(GPIOD, ButtonB, TM_GPIO_Mode_IN, TM_GPIO_OType_PP, TM_GPIO_PuPd_DOWN, TM_GPIO_Speed_Low); TM_ADC_Init(ADC3, ADC_Channel_4); } void startPWM(void) { //PWM rigth Turn TM_PWM_InitTimer(TIM2, &TIM2_Data, 1000); TM_PWM_InitChannel(&TIM2_Data, TM_PWM_Channel_1, TM_PWM_PinsPack_2); TM_PWM_SetChannelMicros(&TIM2_Data, TM_PWM_Channel_1, 1000); //PWM left Turn TM_PWM_InitTimer(TIM3, &TIM1_Data, 1000); TM_PWM_InitChannel(&TIM1_Data, TM_PWM_Channel_2, TM_PWM_PinsPack_1); TM_PWM_SetChannelMicros(&TIM1_Data, TM_PWM_Channel_2, 1000); } //static int buttonCareer(void) //{ // int8_t val = 0; // if (TM_GPIO_GetInputPinValue(GPIOD, ButtonA) == 1 || TM_GPIO_GetInputPinValue(GPIOD, ButtonB) == 1) // val = 1; // else 112 D.2 accions.c // val = 0; // // return val; //} int openCloseGripper(uint16_t valVel, uint16_t valRef) { float uk, valPos; valPos = (4096*valRef)/170; State = 1; CustomTimer1 = TM_DELAY_TimerCreate(1, 1, 1, CustomTIMER1_Task, NULL); while (State == 1) { uk = PIDControl(valPos, valSensorPos, 0); if (uk > 0) { if (uk > valVel){ uk = valVel;} TM_PWM_SetChannelMicros(&TIM2_Data, TM_PWM_Channel_1, (1000-uk)); TM_PWM_SetChannelMicros(&TIM1_Data, TM_PWM_Channel_2, 1000); } else { if (uk < -valVel){ uk = -valVel;} TM_PWM_SetChannelMicros(&TIM1_Data, TM_PWM_Channel_2, (1000+uk)); TM_PWM_SetChannelMicros(&TIM2_Data, TM_PWM_Channel_1, 1000); } } return 2; } /* Para que se mueva a la velocidad deseada se tiene que restar la accion a 1000 es decir (1000 - u)*/ void startADC(void) { TM_ADC_Init(ADC1, ADC_Channel_0);//Finger Sensor TM_ADC_Init(ADC1, ADC_Channel_3);//Finger Sensor //TM_ADC_Init(ADC1, ADC_Channel_7);//Finger Sensor TM_ADC_Init(ADC1, ADC_Channel_8);//Finger Sensor TM_ADC_Init(ADC1, ADC_Channel_9);//Finger Sensor TM_ADC_Init(ADC1, ADC_Channel_10);//Finger Sensor TM_ADC_Init(ADC1, ADC_Channel_11);//Finger Sensor TM_ADC_Init(ADC1, ADC_Channel_12);//Finger Sensor TM_ADC_Init(ADC3, ADC_Channel_4);//Position Sensor } void readADC(int8_t val) { float result1 = 0; float result2 = 0; float result3 = 0; float result4 = 0; float result = 0; switch(val) { case 0: //Two internal fingers result1 = TM_ADC_Read(ADC1, TM_ADC_Channel_9); 113