PROYECTO ROBOT MENTOR_V1_ENERO_19_por_Joaquin Berrocal Piris

Proyecto: AUTOMATIZACIÓN DEL ROBOT MENTOR (Por Joaquín berrocal piris)

Puede ser manejado desde: +La Caja Controladora, mediante Joysticks +desde el Mando de la PS2X, o +desde el PC, mediante programa creado en VB 2017 Nombre del proyecto (en ARDUINO V 1.6.4): “Brazo_robot_MENTOR_Joaquin_Berrocal_Piris_Nov_18_V1.ino” Autor: Joaquín Berrocal Piris Fecha : Enero 2019

COMPONENTES: + 6 MOTORES DC. (Alimentados a 9V) + 6 POTENCIÓMENTROS PARA CONOCER POSICIONES (2K) + 3 BRAZOS + PINZA Permite la grabación de los movimientos EN TARJETA SD y la secuenciación de los mismos.

En el comienzo tengo una descripción con las fotos del proceso de CONSTRUCCIÓN. De la 41 a 43 el tutorial de uso de los drivers L298N Al final, desde la página 44 a la 97, tengo escrito todo el programa realizado en Arduino 1.6.4. + Dispone de Fuente de alimentación ajustada a 14,7V (igual tensión que tenía la F.A. original del robot Mentor) y luego mediante regulador STEP UP/DOWN Xl6019 (20W / 5-32V / 1,5-35V ) ajusto y estabilizo tensión a 9V DC para alimentar: a la placa Arduino y a los Drivers L298N que a su vez alimentan a los motores.

Con esta tensión No se calienta apenas la placa arduino.. /*proyecto: "Brazo_robot_MENTOR_Joaquin_Berrocal_Piris_Nov_18_V1.ino" Autor: JOAQUÍN BERROCAL PIRIS Fecha: Nov 2018 - ENERO 2019 Está basado en el programa "Arduino_brazo_robot_PC_VB2015_JBP_V4.ino" de mi proyecto de Enero de 2017 del brazo robótico KSR10 DE VELLEMAN. Dirección del video: https://www.youtube.com/watch?v=SFNrReSie1o&t=303s Dirección de mi canal: https://www.youtube.com/user/joaquininbp/videos?sort=dd&shelf_id=0&view=0 o simplemente poner mi nombre completo y saldrán en primera posición mis videos y archivos subidos a youtube -- slideshare -- issuu Dirección de issuu del brazo robotico anterior: https://issuu.com/joaquinin/docs/brazo_robotico_ksr10_ps2x__mayo_201 Dirección global de mis archivos subidos a issuu: https://issuu.com/joaquinin Dirección en issuu de este proyecto: https://issuu.com/joaquinin/docs/proyecto_robot_mentor_v1_enero_19_p Dirección en slidesahre https://www.slideshare.net/joaquinin1/brazo-robotico-ksr10-ps2-x-mayo-2015-a-9vversion3 Dirección global en slideshare https://www.slideshare.net/joaquinin1/documents +++++++El robot puede ser manejado desde: ++++++++ a) Desde los Joystick de la mesa o caja controladora (pin 12 a nivel 0) b) Desde la PlayStation PS2X (pin 12 a nivel 1)(Me sirvo de la librería "#include <PS2X_lib.h> " adaptándola a mi mando ps2x, que no es original al de SONY aunque sí compatible; precio 14,92$.) c) Desde el PC con el programa hecho en VB2017 (pin 13 a nivel 0) La selección del modo de trabajo se realiza mediante 2 conmutadores; Si se selecciona en modo "PC" deberá estar la placa arduino conectada mediante cable USB al programa que he realizado en VB2017 para su control. Éste modo tiene prioridad sobre los otros dos.

------------------------NOTAS--------------------------En mi PS2X chino debo hacer el ps2x.config_gamepad(…) doblemente para que sea reconocido el módulo receptor ps2x, lo que descubrí, después de muchas pruebas, en las que el módulo receptor no era siempre reconocido. Además, también pongo el pin "acknoledge" a masa. si lo dejo al aire, se dificulta el reconocimiento del módulo.

Dejaré información sobre el mando PS2X más adelante con dibujos y valores de sus mandos.

En el "Setup ()" poner dos veces ; //1ª ) setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error error = ps2x.config_gamepad (14,15,16,17,true,true); delay(50); aquí puedo bajar incluso suprimir este delay(50) //2ª) setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error error = ps2x.config_gamepad(14,15,16,17, true, true); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

1

delay(50); // este es necesario aunque sí acepta menos tiempo. //---------------------------------------------------------------------- Entradas Analógicas de Control-----------MOTOR PINANALOGICO POTX ACCIÓN M1_D A0 A6 Sube/baja pinza (Activa M1_D+ M1_I) M1-I A1 A7 Rota pinza M1_A_C A2 A8 Abre-Cierra Pinza M2 A3 A9 Motor 2 sube-baja M3 A4 A10 Motor 3 sube Baja M4 A5 A11 Motor 4 de columna-base ------------------- FUNCIONAMIENTO ----------------------------------------BREVE EXPLICACIÓN: -------------------El proyecto está realizado sobre una ARDUINO MEGA 2560 por necesitar más pines que los de la UNO 3. Consiste en controlar de forma automática, mediante joystick, el movimiento de un brazo robótico de 6 motores con dos brazos, más una columna giratoria + pinza manejada desde dos motores con engranaje diferencial.

Los motores los alimento a 9V DC, pero acepta sin problemas, por encima de los 12V. El único problema es el calentamiento del regulador de tensión de la placa Arduino que a mayor tensión mayor es la caída de tensión que debería absorber y, por tanto, transformar en calor, pues como sabemos, esta trabaja a 5 voltios.

Es por lo que trabajo a 9 voltios para alimentar a la placa Arduino. Tensión constante y regulados mediante un STEP UP/DOWN XL6019.

Es fundamental que la tensión no tenga variaciones pues, de lo contrario, los cálculos de posición y activación de motores sería muy irregular e impredecible. + Dispone de un interruptor para permitir, si lo deseamos, grabar los movimientos que realice y serán grabados sobre tarjeta SD.. ""El momento de permitir la grabación será indicado con led verde"". + Dispone también de un interruptor para permitir, si así queremos, secuenciar los movimientos grabados de forma continua mientras se mantenga activado.

Será indicado mediante led Azul. +Dispone de un interruptor para hacer el control, o bien, desde el mando de la PS2X (cuando está en (+) el pin 12 de arduino, o bien desde los Josystick de la caja de control, cuando el pin 12 está a masa (-) +Dispone de otro interruptor, en este caso con prioridad sobre el anterior, para el manejo del robot desde el PC con programa hecho en Visual Basic 2017 + Se pueden memorizar los movimientos en una tarjeta microSD y luego secuenciarlos con ayuda de los interruptores correspondientes de "Grabar" y "Secuenciar" Pero si la Grabación la hacemos desde el PC, Los movimientos realizados desde el visual estudio se memorizan, no en la tarjeta SD, se hace en un archivo de texto, que se guarda en mis documentos\*.txt y se ve reflejado en el datagrid del programa; permite guardar hasta 25 posiciones.

IMPORTANTE: ESTE ARCHIVO DE TEXTO no debe tener más de ¡¡8 caracteres!!

"TODOS ESTOS INTERRUPTORES con su palanca HACIA ABAJO están a nivel ALTO"

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

2

+ Cuando se activa el programa, al igual que cuando se activan los interruptores de grabar o secuenciar, lo primero que hará será ir a posición HOME. que corresponde a posición de 90º en los potenciómetros de los respectivos motores. + Si se pulsa el interruptor de Grabar o secuenciar; esperar a que se encienda el led verde para permitir grabar los movimientos o el led Azul para q se secuencien los movimientos grabados en la SD. + El proyecto está comunicado por el USB pines 0-1 para comunicar a través de puerto serial mensajes varios entre ellos datos de posición. "lo tengo como ayuda para ajustar el proyecto".

FINALMENTE EL PROYECTO HA QUEDADO BASTANTE AJUSTADO Y CALIBRADO con sus pequeñas desviaciones debido a las holguras de engranajes/potenciómetros e histéresis dadas para precisión y calibración. El desajuste aproximado es de unos +- 2º.

Utilizo la librería Proporcional Integral y Derivativa ; "PID_v1.h" para el control de movimientos de los motores, pero sólo para el manejo y control desde el Visual Basic 2017.

No obstante, para la posición a HOME no la utilizo, porque consigo mejores resultados haciéndolo de forma tradicional leyendo los valores analógicos de los potenciómetros de posición. ++++++++++++++++++ MATERIALES Y CONEXIONES: ++++++++++++++++++ -. 6 Motores a 9 voltios DC -. 3 ejes -. 6 Potenciómetros (2K)de control posición ****CONEXIÓN ANALOGICA DE LOS Joystick y Potenc****** Joystick ---- Potenciometro MOTOR PINANALOGICO POTX ACCIÓN M1_D A0 A6 Sube/baja pinza (Activa M1_D+ M1_I) M1-I A1 A7 Rota pinza M1_A_C A2 A8 Abre-Cierra Pinza M2 A3 A9 Motor 2 sube-baja M3 A4 A10 Motor 3 sube Baja M4 A5 A11 Motor 4 de columna-base ***************************************************** -. 3 Amplificadores de potencia L298N que utilizo para alimentar y cambiar direción de giro de los motores, admiten hasta 2 Amp con puntas de hasta 4A por canal (realmente estos 6 motores, en conjunto, no llegan a a sobrepasar 1,5 A.) -. tarjeta SD 8GB para grabación de las secuencias de movimientos que deseo memorizar microSD Card Adapter . Pin CS de la Tarjeta al pin 53 de la Arduino Mega 2560 Si utilizase la arduino Uno sería al pin 10 y en la ethernet el 4 conexión pines Tarjeta SD; Ardu Mega Ardu UNO CS ---------- 53 ------ 10 "en la ethernet shield el 4" clk/sck ---- 52 ------ 13 MOSI ------ 51 ------ 11 MISO ------ 50 ------ 12

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

3

-. Conexión de los Drivers amplificadores y de los Joystick a la arduino Mega: Nº Nº 1 Nº 2 Nº 3

DRIVER L298N Motor M1_D (out 1-2) Motor M1_I (out 3-4) Motor M2 (out 1-2) Motor M3 (out 3-4) Motor M1_A_C ( 1-2) Motor M4 (out 3-4) M1_D + M1_I Rotar

Joystick Nº1 (A0) (A1) Nº3 (A3) Nº3 (A4) Nº2 (A2) Nº4 (A5) Nº1 (A1)

*Joystick* A0 Nº 1 Nº 2 Eje (X) Nº 3 Nº 4 Eje (X)

PWM JOYS PotX IN1 2 A0 A6 22 3 A1 A7 4 A3 A9 26 5 A4 A10 6 A2 A8 30 44 A5 A11 2-3 A1 A7 22

IN2 IN3 IN4 Letra 23 A 24 25 B 27 D 28 29 E 31 C 32 33 F 23 24 25 B

A1

Función Activa a la vez M1_D y M1_I (motores de la pinza) Activa M1_D y M1_I (Rotar pinza) Sólo utiliza eje Horizontal

A2

Activa M1_A_C (abrir cerrar pinza)

A3 A4

Activa M2 ( Sube/Baja brazo 1) Activa M3 ( Sube/Baja brazo 2)

A5

Activa M4 (Gira Dcha-Izqda Base)

//------------------------------------------------------------Conexión del receptor PS2X playStation a la placa arduino mega: //------------------------------------------------------------SE DEBE EFECTUAR A TRAVÉS DE UN CONVERSOR de señal de 5v a 3,3V. Pues LA PLACA ARDUINO TRABAJA A 5V y la playstation trabaja a 3 Voltios.

IMPORTANTE. conectar a dicha placa los 3,3 en su lado de nivel bajo y de ahí al módulo receptor PS2X y en el lado de alta alimentarlo a 5V. setup pins and settings: ps2x.config_GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error error = ps2x.config_gamepad(14,15,16,17, true, true); pin Receptor PS2X pin Arduino 7 ---- CLK (clok) --------------- 34 (14) 2 ---- CMD (comand) -------- 35 (15) 6 ---- ATT (attention) -------- 36 (16) [EL ATT también se llama CS (Chip Select)] 1 ---- DAT (data) -------------- 37 (17) 5 ---- VCC 3,3V ------------ pin de 3,3V --> conectarlo al conversor de señal y y al receptor PS2X 4 ---- GND masa ------------ GND 9 ---- Acknolage ----------- a GND IMPORTANTE si lo dejo al aire problemas reconocimiento del módulo "N/C no conexinados los pines 3 y 8 del módulo recepetor."

Nota: NO USO EN ESTE PROYECTO los pines 14,15,16,17 por problemas con la soldadura de la placa shield que utilizo. Empleo los pines 34,35,36,37. error = ps2x.config_gamepad(14,15,16,17, true, true); poner en su lugar (34,35,36,37) NOTA el último pin de config_gamepad (x,x,x,y) //la librería la debe configurar como entrada he hecho pruebas. *************MANDO PLAYSTATION PS2X. OBSERVACIONES:************************************ -. En la lectura analogica de los joystick del mando de la playStation se leen Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

4

Tope hacia adelante; valor 0. Tope hacia atrás 255 ("y NO 1023") Tope a la Izqda ; valor 0 . Tope hacia la Dcha 255 ("y NO 1023") Ejemplo de lectura analógica: donde se muestra que la lectura va de 0 a 255 y la convierto en 180 a 0 servoVal = map(ps2x.Analog(valorPSS),0,255,180,0); //Así se lee el valor analógico del mando ps2x **************************************************************************************** ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ El mando PS22X tiene 13 pulsadores y dos JOYSTICK con 2 pulsadores más y cada uno tiene un nombre descriptivo y su valor decimal. Indicaré el valor decimal de aquellos que he utilizado en este proyecto para manejo de los motores.

Nombre (valor decimal) (Más adelante, dejaré imagen del mando con los valores) PSB_L1 PSB_PAD_UP >>>>(16) y como valor analógico: PSAB_PAD_UP >>>>(11) PSB_L2 PSB_PAD_DOWN >>>>(64) y como valor analógico: PSAB_PAD_DOWN >>>>(12) PSB_R1 PSB_PAD_LEFT >>>>(32) y como valor analógico: PSAB_PAD_LEFT >>>>(9) PSB_R2 PSB_PAD_RIGHT >>>(128) y como valor analógico: PSAB_PAD_RIGHT >>>(10) PSB_SELECT PSB_START PSB_MODE PSB_BLUE >>>(15) (activa Led iluminación) y como valor analógico: PSAB_BLUE PSB_RED >>>(14) (apaga Led iluminación) y como valor analógico: PSAB_RED PSB_PINK y como valor analógico: PSAB_PINK PSB_GREEN y como valor analógico: PSAB_GREEN Joysticks de la PS2X; PSS_LY (8) utilizado en el proyecto PSS_LX (7) PSS_RY (6) utilizado en el proyecto PSS_RX (5) ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -. (*los leds usados son de cátodo común (-)) -. ledRojo al pin 9 aviso que se dirige al Home (sucede al activar cualquier interruptor) -. ledVerde al pin 10 se pueden grabar los movimientos. (interruptor grabar LOW) -. ledAzul al pin 11 secuenciar movimientos grabados (interruptor secuenciar LOW) -. led amarillo intenso (R 240 V 245 A 7) mediante: analogWrite (pinPWM, valor de 0 a 255) analogWrite (9,240); analogWrite (10,240) ; analogWrite (11,240); -. El LED de iluminación del Brazo robótico, conectado al pin 43 de arduino -. interruptor para grabar movimientos: al pin 7 -. interruptor para secuenciar movimientos grabados al pin 8 -. interruptor PARA seleccionar los Joystick o bien la PS2X al pin 12 -. Interruptor para ser manejado desde el PC con el Visual Basic 2017 al pin 13 (la resistencia de TODOS los interruptores puestas a + Pull-up al activar el pulsador conecto el pin 7 o el 8 o el 12 o el 13 a masa (-)) Con su palanca hacia abajo están a nivel alto y hacia arriba a nivel bajo. ++++++++Niveles TOPES de giro máximo : ++++++++ Están limitados por software para evitar choques y deterioro de Potenciómetros (creo que sólo el Pot del Motor 2 no es multivueltas.) no estoy muy seguro por haberselo cambiado por otro al estar deteriorado. y no habérselo comprobado...

Motor M1_D 120º (680) a Izqda y 56º (320)a Dcha Motor M1_I 56º (320) a Izqda y 119º (680)a Dcha Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

5

Motor M1_A_C 104º (594) Cerrada y 64º (3689 Abierta Motor 2 de 5º (33) hacia Arriba y 107º (611) hacia Abajo Motor 3 de 140º (796)hacia Arriba y 42º(242) hacia Abajo Motor 4 de 148º (842)Izqda y 30º(180)a Dcha. **** OTROS DATOS Interesante en la configuración del programa: ****** -. En la función; int JoystickPS2X (int valorPSS, int pinAnalogico,int pinPWM,int int1,int int2,char letra) tengo el accionamiento manual de los motores más chequeo interruptores de grabar o secuenciar -. En HOME

//Por defecto y como valor global; valorPWM_S = 180; valorPWM_B = 180; Aunque para M2 Y M4, valorPWM_S = 255

-. En JoystickPS2X valor global de accionamiento 200 y 180 -. En ActivarMotores valor global de accionamiento 225 y 200 motor M2 y M4 255 Si deseo incorporar pantalla LCD para reflejar valores y avisos; Para trabajar con La LCD 4X16 utilizar 2 hilos el de reloj SCL y el de Datos utilizar SDA --- 20 SCL -- 21 ("En este proyecto no la utilizo")

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

6

PROCESO DE CONSTRUCCIÓN ROBOT MENTOR V1 (Nov 2018 a Enero 2019) Vídeo en youtube de éste y otros proyectos míos: https://www.youtube.com/user/joaquininbp/videos?view=0&shelf_id=0&sort=dd ESTADO INICIAL Dañados: Pinza rota / Potenciómetros / engranajes...

ESTADO FINAL REFORMADO

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

7

LOS CONECTORES DE LOS MOTORES NO DISPONÍAN DE CONDENSADORES DE PROTECCIÓN PARA ABSORBER FLUCTUACIONES DE TENSIÓN Y RUIDOS ELÉCTRICOS QUE SE PODRUCEN EN SUS ESCOBILLAS DE ALIMENTACIÓN. Yo les pongo a cada motor condensadores de tantalio de 100 nF. (2A104J)

Sin condensador

Con condensador 100 nF

Potenciómetro para cálculo posición Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

8

Los brazos no giraban, estaban bloqueados por gripado del conjunto de engranajes de las reductoras de cada motor. Se tuvo que desmontar y limpiar.

ARREGLO Y PUESTA A PUNTO DE LOS ENGRANAJES

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

9

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

10

VERIFICACIÓN DE QUE GIRAN CORRECTAMENTE LOS MOTORES

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

11

(MOTOR M1_A_C) Permite Abrir y Cerrar la pinza La cogida que tiene para el cable trenzado está deteriorada y es muy complicada su fijación.

Si se rompiera de nuevo el cable, sería muy complicado su arreglo.

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

12

DESMONTAJE DE SUS ELEMENTOS ELECTRONICOS QUE TAMBIÉN SE ENCONTRABAN INOPERATIVOS DEBIDO A SU ESTADO. La placa ORIGINAL disponía de dos integrados” Intel” muy potentes los 8085.

EN EL DISEÑO FINAL de renovación del robot MENTOR SE SUSTITUYE la citada placa por una ARDUINO MEGA 2560 R3

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

13

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

14

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

15

ARREGLO Y PUESTA A PUNTO DE LA PINZA

PINZA ESTROPEADA

PINZA ARREGLADA

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

16

CONTRUCCIÓN DE LA CAJA CONTROLADORA PARA LOS JOYSTICK

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

17

INTERIOR DE LA CAJA CONTROLADORA

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

18

MONTAJE FINAL DE LA CAJA CONTROLADORA

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

19

INCIDENCIA GRAVE A LA HORA DE PONER EL BOTÓN DE “RESET” EN LA CAJA. Al hacer el agujero con el taladro, se enrollo la broca con un papel, tiró del cableado y destrozó interruptores, conexión de los leds y partío bastantes cables. UN DESASTRE estando ya todo terminado, probado y verificado. (puede que todavía haya algún cable por descubrir con problemas debido al estiramiento que sufrieron)

Papel al que se unió la broca provocando el problema

Interruptores rotos, cables retorcidos e inservibles (¡¡¡ para tirar la toalla..!!!)

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

20

A ver quién arreglaba esto ahora. ¡¡ Para decir, se acabó y no sigo !!

Pero, después de muchas horas de trabajo, conseguí solucionarlo. Un milagro.

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

21

TRABAJOS DE SOLDADURAS PARA EL MONTAJE Y PRUEBAS INICIALES DE FUNCIONAMIENTO TODO ESTO ANTES DE TENER EL CONECTOR REDONDO DE 26 PINES NI LA PLACA SHIELD

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

22

PRUEBAS SOBRE LA MEGA 2560 R3 (a la espera de recibir la placa shield)

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

23

MONTAJE DE LA PLACA SHIELD PARA LA ARDUINO MEGA interesante por ir atornillados los cables a ella, garantizando una mayor estabilidad en la unión.

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

24

YA MONTADA SOBRE LA ARDUINO MEGA 2560 R3

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

25

COLOCANDO Y PONIENDO A PUNTO LOS MOTORES Y POTENCIÓMETROS DE POSICIÓN

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

26

CONSTRUCCIÓN Y MONTAJE EN LA CAJA DE CONTROL DEL CONECTOR REDONDO MACHO DE 26 PINES

CONSTRUCCIÓN Y MONTAJE DEL CONECTOR REDONDO MACHO DE 26 PINES EN EL ROBOT

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

27

CONSTRUCCIÓN DEL CABLE DE UNIÓN ENTRE CAJA Y ROBOT MENTOR MEDIANTE CONECTORES REDONDO HEMBRA DE 26 PINES.

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

28

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

29

Y POR FIN YA TERMINADO EL CABLE DE UNIÓN ENTRE LOS POTENCIÓMETROS/JOYSTICKS/MOTORES

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

30

SUSTITUCIÓN DE LA FUENTE DE ALIMENTACIÓN ANTIGUA POR UNA REGULABLE DE 12V/10A/120W CON ELLA, SE SUPRIMEN LA PLACA DE LOS CONDENSADORES Y EL TRANSFORMADOR

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

31

PLACA PARA LA GRABACIÓN EN TARJETA MICRO SD DE LOS MOVIMIENTOS REALIZADOS DESDE LA MESA CONTROLADORA. Si el manejo se hace desde el PC, la grabación de los movimientos, se hará en archivo de texto y/o datagrid.

La grabación de los movimientos es siempre opcional y nos permite poder secuenciarlos también cuando deseemos. (El nombre del archivo no debe tener mas de 8 caracteres)

LEER Y ESCRIBIR EN UNA TARJETA SD o MICRO SD CON ARDUINO Información https://www.luisllamas.es/tarjeta-micro-sd-arduino/

conexión lector micro SD, sería la siguiente.

Los pines SPI indicados son válidos para los modelos de Arduino Uno, Nano y Mini Pro. Para otros modelos de Arduino consultar el esquema patillaje correspondiente.

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

32

Arduino pin

Verificado

Pot 1_D salida analógica a A6 Pot 1_I salida analógica a A7 Pot1_A_C abre cierra pinza a A8 Pot 2 salida analógica a A9 Pot 3 salida analógica a A10 Pot 4 salida analógica a A11 ESTA SUELTO DEL C1 soldarlo (-) M1_D de la pinza Dcha a Out2 Driver 1 (+)M1_D de la pinza Dcha a Out1 Driver 1 (-) M1_I de la pinza Izqda a Out4 Driver 1 (+) M1_I de la pinza Izqda a Out3 Driver 1 (-) M2 del Brazo-de Pinza 1 a Out2 Driver 2 (+) M2 del Brazo-de Pinza 1 a Out1 Driver 2 (-) M3 del Brazo 2 a Out4 Driver 2 (+) M3 del Brazo 2 a Out3 Driver 2 (-) M1_A_C de la pinza a Out2 Driver 3 (+) M1_A_C de la pinza a Out1 Driver 3 (-) M4 del Brazo-Columna 3 a Out4 Driver 3 (+) M4 del Brazo-Columna 3 a Out3 Driver 3 23 filas conexiones LED de iluminación del Brazo robótico, conectado al pin 43 de arduino R270 ohm 18mA. Se activa dándole un (+). Uso el cable blanco q me ha sobrado del Ca1.

Nota: utilizo el pin 43 en lugar del 18 como en principio quería, por no tener salida debido, seguramente, a mala soldadura en la placa shield montada sobre la MEGA. LED ROJO RGB LED VERDE RGB LED AZUL RGB +5v Procedente del L298 N1 a ficha empalme interna, para alimentar potenciómetros +14,7 procedente Transformador interno a través de interruptor Power para alimentar drivers L298N caja de control y hacia Step Up/Down para regular a 9V la tensión de alimentación Arduino, así evito que se caliente su Reg. De tensión. He observado que hasta 10V no se calienta mucho la placa arduino.

GND de los 14,7V Transformador interno. (Tengo Todas las Masas Unidas)

√ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √

Color

A6 A7 A8 A9 A10 A11

C. Redondo Borne

CONECTOR DE 26 PINES MACHO DE LA CAJA DE CONTROL Y DEL ROBOT MENTOR FUNCIÓN

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17

V G Na Ma Vi N M R B Vi Na G V G V Am Az

18

Am

19

B/R 43

20 21 22

R V Az RAz

23

24

RAz

25

MN

26

9 10 11 +5v

GND

√

√

√ √ √ √

√

√

NO CONEXIONADO

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

33

Cableado 2

Cableado 3

N Ma R Na Am V Az Vi G B N Ma R Na Am V Az Vi G B N Ma R Na Am V

Verificado

Color

Borne

Manguera Cable Cableado 1

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 (+12v) 25 (-) 26

√ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √ √

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

34

Shield de arduino MEGA-2560 - Prototype Screw/Terminal Block Shield Board Kit For MEGA-2560 R3 Z09 Drop ship--------9,66€ A 17 DE OCT DE 2018------------Esquema interno de la placa shield de arduino MEGA 2560: el esquema interno Top y botton CARA TOP

CARA BOTTOM

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

35

--------------------------ADUINO MEGA 2560 R3 ----------------------------

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

36

PATILLAJE DE DISTINTAS PLACAS ARDUINO: https://www.luisllamas.es/esquema-de-patillaje-de-arduino-pinout/

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

37

LEER Y ESCRIBIR EN UNA TARJETA SD o MICRO SD CON ARDUINO Información https://www.luisllamas.es/tarjeta-micro-sd-arduino/

conexión lector micro SD, sería la siguiente.

Los pines SPI indicados son válidos para los modelos de Arduino Uno, Nano y Mini Pro. Para otros modelos de Arduino consultar el esquema patillaje correspondiente.

Mando y receptor wifi de la playStation PS2X

CONVERSOR DE 5 V A 3 V. Por otro lado, en la mayoría de los casos necesitaremos un conversor lógico de nivel de 3,3V a 5V. El motivo es que, por lo general, nuestro Arduino opera a 5V, mientras que la electrónica del mando de la PS2 funciona a 3,3V. Podríamos hacer nuestro propio circuito de adaptación, pero no tienen ningún sentido dado que se venden por 1,10€, gastos de envío incluidos.

Para encontrar el conversor simplemente buscar “level converter” en Ebay. Estamos buscando un artículo similar al siguiente “JY MCY I2C LEVEL CONVERTER”, un dispositivo que permite adaptar 4 señales de forma bidireccional. Fijaros que el modelo que queremos tiene 6 pines (no compréis uno de 4 pines, que solo pueden adaptar 2 señales).

El Acknolage CONECTARLO A MASA para que se reconozca fácilmente el módulo receptor inalámbrico

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

38

CONEXIÓN DEL MÓDULO RECEPTOR WIFI DE LA PS2X Y EL CONVERSOR DE 5 A 3,3V A continuación usamos una protoboard para realizar las conexiones con nuestro conversor digital (o lo soldamos directamente). En el lado de baja tensión se conectan las cuatro señales del mando, ground, y alimentación, que será proporcionada por Arduino. En el lado de alta tensión conectamos cuatro pines, ground, y +5V que será convertido por el conversor a +3,3V para alimentar el receptor.

El esquema definitivo es el siguiente. Acknolage a masa Y el terminal de 3 V del convertidor unirlo también con los 3,3V de la placa arduino, al menos en mi convertidor comprado que no es exactamente igual a éste

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

39

Esquema Mando PS2X de la PlayStation con el nombre que reciben sus pulsadores y Joystick (Este no es inalámbrico como el que tengo instalado pero me vale su descripción) INFORMACIÓN en internet : "Luis Llamas"

Como valor analógico: PSAB_PAD_UP valor 11 PSAB_PAD_DOWN valor 12 PSAB_PAD_LEFT valor 9 PSAB_PAD_RIGHT valor 10

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

40

TUTORIAL USO DRIVER DUAL L298N PARA MOTORES DC y paso a paso ESTA ES LA MISMA PLACA QUE COMPRÉ EN ALIEXPRESSS.

http://electronil b.co/tutoriales/tutorial-de-uso-driver-dual-l298n-para-motores-dc-y-paso-a-paso-con-arduino/

Tutorial: Uso de Driver L298N para motores DC y paso a paso con Arduino El siguiente tutorial está basado en el Driver dual para motores (Full-Bridge) – L298N, ofrecido por ELECTRONILAB.CO. Puedes adquirir este módulo en nuestra tienda.

Este módulo basado en el chip L298N te permite controlar dos motores de corriente continua o un motor paso a paso bipolar de hasta 2 amperios.

El módulo cuenta con todos los componentes necesarios para funcionar sin necesidad de elementos adicionales, entre ellos diodos de protección y un regulador LM7805 que suministra 5V a la parte lógica del integrado L298N. Cuenta con jumpers de selección para habilitar cada una de las salidas del módulo (A y B).

La salida A esta conformada por OUT1y OUT2 La salida B por OUT3 y OUT4.

Los pines de habilitación son ENA y ENB respectivamente. deben estar a nivel alto para estar operativo Echemos un vistazo a cómo controlar sólo uno de los motores, Motor1. Con el fin de activar el motor, la línea ENABLE1 debe ser alta. A continuación, controlar el motor y su dirección mediante la aplicación de una señal LOW o HIGH a las líneas Input1 y INPUT2, como se muestra en esta tabla.

Input1 0 1 0 1

Input2 0 0 1 1

Acción Parada del motor El motor gira hacia adelante El motor gira hacia atrás Parada del motor

En la parte inferior se encuentran los pines de control del módulo, marcados como IN1, IN2, IN3 e IN4.

Conexión de alimentación Este módulo se puede alimentar de 2 maneras gracias al regulador integrado LM7805.

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

41

√ Cuando el jumper de selección de 5V se encuentra activo, el módulo permite una alimentación de entre 6V a 12V DC. Como el regulador se encuentra activo, el pin marcado como +5V tendrá un voltaje de 5V DC. Este voltaje se puede usar para alimentar la parte de control del módulo ya sea un microcontrolador o un Arduino, pero recomendamos que el consumo no sea mayor a 500 mA. √ Cuando el jumper de selección de 5V se encuentra inactivo, el módulo permite una alimentación de entre 12V a 35V DC. Como el regulador no está funcionando, tendremos que conectar el pin de +5V a una tensión de 5V para alimentar la parte lógica del L298N. Usualmente esta tensión es la misma de la parte de control, ya sea un microcontrolador o Arduino.

-------------------------------------------------- 0 -----------------------------------------------http://electronilab.co/tienda/driver-dual-para-motores-full-bridge-l298n/ Descripción del Producto Este módulo es el complemento ideal para proyectos de robótica y Router CNC.Permite controlar hasta 2 motores de corriente continua o un motor paso a paso bipolar. También permite controlar un motor paso a paso unipolar configurado como bipolar de forma muy sencilla y eficaz. .

Características • Voltaje de alimentación, mínimo de 5 V. Posee dos entradas, una de 5V para controlar la parte lógica y otra para alimentar las salidas al motor, que pueden ser de 5V o más.

• La tarjeta tiene la opción de habilitar un regulador LM7805 integrado en ella para alimentar la parte lógica con lo que se puede alimentar la tarjeta con 12V por ejemplo.

• Corriente máxima 2 Amperios.

• Posee 6 entradas de control (ver tabla de control) • Admite entradas de señal PWM para el control de velocidad.

• Dimensiones: 43 mm x 23,9 mm x 43 mm.

• Salidas: para 2 motores de DC o para un motor bipolar paso a paso.

Partes

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

42

ESQUEMA L298N :

*/

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

43

//+++++++++++++++ COMIENZO DEL PROGRAMA EN ARDUINO V1.6.4 +++++++++++++++ //#include <SPI.h> en el portátil que tengo instalado el VB15, Arduino, me pide esta librería para compilar el programa y no lo entiendo pero es así #include <SD.h> #include <PID_v1.h> //**para control PID de los Motores al ser controlados por VB17 #include <string.h> //**manejo de string File miarchivo; // -----CONFIGURACIÓN DE PINES y VARIABLES GLOBALES---const int CS_SD = 53; //53 habitual de las MEGA. en Arduino UNO sería el 10 //sin embargo lo llamo en el proyecto por el número 53 int servoVal,valorPotX,pinAnalog,valorA0,valorA1; // variables a leer por los puertos analógicos int valorAnterior; int valorPWM_S = 200; //valor global para Subir los motores int valorPWM_B = 180; //valor global para Bajar los motores byte Interruptor_grabar = 7; //el pin 7 para grabar si está a masa byte Interruptor_secuenciar = 8; //el pin 8 a nivel 0 para secuenciar el movimiento grabado byte Interruptor_PS2X_Joystick = 12;//nivel 1 manejo con los Joystick de la mesa // nivel 0 manejo con el mando de la playStation PS2X byte Interruptor_VB17 = 13; //++++el pin 13 a nivel 0 se MANEJA con el Visual Basic 2017 int verde = 10; //pin 10 Se puede Grabar int rojo = 9; //pin 9 mientras se va al home al pulsar interruptores de Grabar o Secuenciar. int azul = 11; //pin 11 para secuenciar_movimientos //Para activar el led de iluminación del brazo robótico. Se activa //desde la PS2X al pulsar la tecla azul (X) y se desconecta al pulsar la tecla roja (O) int led = 43; //NO USO EL 18 POR NO TENER EN MI PLACA SHIELD bien la SALIDA 18 ni la 19, seguramente por mala soldura de dichos pines.; byte pulsador_joystick4 = 38; //pin de lectura para activar o no al led de iluminación desde la mesa byte led_verde_bajo = 39; //este led es el que está bajo el "Interruptor_VB17", lo uso para indicar operatividad desde caja o bien desde ps2x //realmente no me era necesario pero al haber hecho sin querer agujero tuve que ponerlo para no afear la caja. byte activar_led_uso_ps2x = 40; //nota lo activa por masa.Los demás leds de los interruptores se activan desde los propios interruptores. bool flag_Led = 0; //para conocer si está activo o no y poder desde la mesa controladora con el PULSADOR //del Joystick 2 encenderlo o bien apagarlo según se encuentre. bool estado_anterior = 0; //**------Declaración de constantes----#define BUFFSIZ 500 //450 NOTA PARA LOS 6 MOTORES 500 y para sólo 5 como en mi anterior version 450 #define ORDENSIZ 8 //7 pARA LA VERSION DE 6 MOTORES 8 Y PARA LA DE 5 PONER 7 #define INSTRUCCIONES 25 // SI NO LO HAGO AL GIRAR EL ÚLTIMO MOTOR AL HACER LA SECUENCIA AUTOMÁTICA NO PARARÁ. //------------------------------------//Declaración de limites de funcionamiento de los distintos ejes //Es posible modificar los parametros siempre que no superen los límites //de accion de los ejes Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

44

//--MOTOR M1_D-#define M1MAX 680 //120ยบ #define M1MIN 340 // 60ยบ //--MOTOR M1_I-#define M2MAX 680 //120ยบ #define M2MIN 340 // 60ยบ //--MOTOR 1_A_C-#define M3MAX 400 // 70ยบ Abierta #define M3MIN 634 // 112ยบ Cerrada //--MOTOR M2-#define M4MAX 33 //6ยบ Arriba #define M4MIN 611 //108ยบ Abajo //--MOTOR M3-#define M5MAX 796 //140ยบ) Arriba #define M5MIN 242 // 43ยบ Abajo //--MOTOR M4-- // El proyecto MENTOR tiene 6 motores LE Aร ADO EL M4 que hacen 6 motores. #define M6MAX 842 //148ยบ A Dcha #define M6MIN 180 // 32ยบ A Izqda /* Nota los valores en la rotaciรณn de pinza que uso en VB17 son M1_I< 780 (137ยบ) Y M1_I > 208 (37ยบ) y no utilizo #definiciรณn dese la mesa controladora no uso topes pues por defecto estรก restringido */ //----------------------------------//------PINES CONTROL L298N puente en H 2 Amp por canal // ----------------Driver D1 manejado desde Joystick 1-------------//motor M1_D motor Dcho de pinzas const int ENA_1 = 2; //pin 2 pulsos PWM control de velocidad const int IN1_1 = 22; //pin 22-53 nivel 1 o 0 para cambiar sentido giro const int IN2_1 = 23; // motor M1_I ---> motor Izqdo de pinzas const int ENA_2 = 3;//pin 3 pulsos PWM control de velocidad const int IN1_2 = 24;//pin 24-55 nivel 1 o 0 para cambiar sentido giro const int IN2_2 = 25; //----------------------------------------// ----------------Driver D2 manejado desde el Joystick 3-------------// motor M2 --> Sube Baja brazo de pinza; Brazo 1 const int ENA_3 = 4;//pin 4 pulsos PWM control de velocidad const int IN1_3 = 26;//pin 26-57 nivel 1 o 0 para cambiar sentido giro const int IN2_3 = 27; // motor 3 M3---> Sube Baja brazo de la columna; Brazo 2 const int ENA_4 = 5; //pin 5 pulsos PWM control de velocidad const int IN1_4 = 28; //pin 28-59 nivol 1 o 0 para cambiar sentido giro const int IN2_4 = 29; //---------------Driver D3 manejado desde el Joystick 1 y 4----------// motor M1_A_C ---> Base const int ENA_5 = 6; //pin 6 pulsos PWM control de velocidad const int IN1_5 = 30; //pin 30-31 nivel 1 o 0 para cambiar sentido giro const int IN2_5 = 31; // motor M4 ---> Base Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquรญn Berrocal Piris. Enero 2019

45

const int ENA_6 = 44; //pin 44 pulsos PWM control de velocidad const int IN1_6 = 32; //pin 32-33 nivel 1 o 0 para cambiar sentido giro const int IN2_6 = 33; //--------------------------------------------------------------------int contaje = 0; //para poner el led verde en HOME //Declaración de Pines para los sensores de posición //----M1_D----int sensorPin1 = A6; int sensorValue1 = 0; int mem_sensor1=0; //----M1_I----int sensorPin2 = A7; int sensorValue2 = 0; int mem_sensor2=0; //----M1_A_C----int sensorPin3 = A8; int sensorValue3 = 0; int mem_sensor3=0; //-------M2-----int sensorPin4 = A9; int sensorValue4 = 0; int mem_sensor4=0; //-------M3-----int sensorPin5 = A10; int sensorValue5 = 0; int mem_sensor5=0; //-------M4-----int sensorPin6 = A11; int sensorValue6 = 0; int mem_sensor6=0; //--------------//Declaración de arrays automatización int pos[INSTRUCCIONES][ORDENSIZ]; char buffer[BUFFSIZ]; //Declaración de variables genericas y de control de procesos int var,var_old,var2; int control=1; int total_progs=0; String cadena; int dato_puerto_serial; int intervalo=0; int control_modo=0; // 0 manual; 1 auto int control_prog=0; //Declaración de otras variables double Setpoint1, Input1, Output1,Setpoint_old1; double Setpoint2, Input2, Output2,Setpoint_old2; double Setpoint3, Input3, Output3,Setpoint_old3; double Setpoint4, Input4, Output4,Setpoint_old4; double Setpoint5, Input5, Output5,Setpoint_old5; double Setpoint6, Input6, Output6,Setpoint_old6;

//M1_D //M1_I //M1_A_C //M2 //M3 //M4

//Declaración de parametros de PID. Es posible modificarles de acuerdo //a las necesidades de velocidad o precisión del brazo robot double aggKp1=6, aggKi1=0.1, aggKd1=0; // 6 0.1 Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

46

double consKp1=0.2, consKi1=0.05, consKd1=0; // 0.2 0.05 double aggKp2=6, aggKi2=0.1, aggKd2=0; // 6 0.1 double consKp2=0.6, consKi2=0.05, consKd2=0; // 0.6 0.05 double aggKp3=2000, aggKi3=0.1, aggKd3=0; // 8 0.1 double consKp3=0.3, consKi3=0.05, consKd3=0; // 2.2 0.05 double aggKp4=2000, aggKi4=0.1, aggKd4=0; // 8 0.1 double consKp4=0.3, consKi4=0.05, consKd4=0; // 0.2 0.05 double aggKp5=100, aggKi5=0.1, aggKd5=0; // 8 0.1 double consKp5=0.2, consKi5=0.05, consKd5=0; // 0.2 0.05 double aggKp6=100, aggKi6=0.1, aggKd6=0; // 8 0.1 double consKp6=0.2, consKi6=0.05, consKd6=0; // 0.2 0.05 PID myPID1(&Input1, &Output1, &Setpoint1, consKp1, consKi1, consKd1,DIRECT); PID myPID2(&Input2, &Output2, &Setpoint2, consKp2, consKi2, consKd2,DIRECT); PID myPID3(&Input3, &Output3, &Setpoint3, consKp3, consKi3, consKd3,DIRECT); PID myPID4(&Input4, &Output4, &Setpoint4, consKp4, consKi4, consKd4,DIRECT); PID myPID5(&Input5, &Output5, &Setpoint5, consKp5, consKi5, consKd5,DIRECT); PID myPID6(&Input6, &Output6, &Setpoint6, consKp6, consKi6, consKd6,DIRECT); //----------------------------------------------------------------

//M1_D //M1_I //M1_A_C //M2 //M3 //M4

//--------------FLAGS----byte FlagGrabar = 0; byte FlagSecuenciar = 0; char FlagRepeticionCicloAuto ; //si utilizo 'a' se repetirá el ciclo del datagrid y si es distinto sólo se repite una vez. int FlagTiempoCiclo = 0; // utilizo para tiempo en repetir ciclo si 0 es continuo sin esperas //----------------------int i = 0; //variable global int valorInt ; //para convertir los valores de los grados a enteros y activar el motor char c ; //char letra; //la hago global en esta Version 1 de Robot MENTOR, para que mantenga su valor al salir de la función int JoystickPS2X (...)

String valorString = ""; //-----------------------// -------Librería para el mando PS2X --------#include <PS2X_lib.h> //for v1.6 /****************************************************************** * set pins connected to PS2 controller: * - 1e column: original * - 2e colmun: Stef? * replace pin numbers by the ones you use ******************************************************************/ /* #define PS2_CLK 14 //13 //12 //17 #define PS2_CMD 15 //11 //11 //15 #define PS2_SEL 16 //10 //10 //16 #define PS2_DAT 17 //12 //13 //14 */ /****************************************************************** * select modes of PS2 controller: * - pressures = analog reading of push-butttons Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

47

* - rumble = motor rumbling * uncomment 1 of the lines for each mode selection ******************************************************************/ //#define pressures true //#define rumble true PS2X ps2x; // create PS2 Controller Class //right now, the library does NOT support hot pluggable controllers, meaning //you must always either restart your Arduino after you conect the controller, //or call config_gamepad(pins) again after connecting the controller. int error = 0; byte type = 0; byte vibrate = 0; int le = 0; //-----------------------------------//+++++CONJUNTO DE RUTINAS EMPLEADAS+++++++ //+++++++++++++++++++++++++++++++++++++++++ void ledRojo() //activa led rojo { digitalWrite(rojo,HIGH); //se enciende aviso movimiento a HOME digitalWrite(verde,LOW); //se enciende permite grabar movimientos digitalWrite(azul,LOW); //se enciende para indicar secuencia de movimientos } void ledVerde() //activa led Verde; mando operativo. { digitalWrite(rojo,LOW); digitalWrite(verde,HIGH); digitalWrite(azul,LOW); } void ledAzul() //activa led Azul { digitalWrite(rojo,LOW); digitalWrite(verde,LOW); digitalWrite(azul,HIGH); } void apagarLeds() //activa led Azul { digitalWrite(rojo,LOW); digitalWrite(verde,LOW); digitalWrite(azul,LOW); } //------------------------------------------------------------------// ++++++FUNCIĂ&#x201C;N PARA EL CONTROL CON EL MANDO DE LA PS2X +++++++++++ //------------------------------------------------------------------//------------------------------------------------------------------//------FUNCION "JoystickPS2X" para manejo manual motores ----------//------bien desde la PS2X o bien desde la MESA de control----------//-------------------------------------------------------------------

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

48

int JoystickPS2X (int valorPSS, int pinAnalogico,int pinPWM,int int1,int int2,char letra) { //+++++++++ Si el interruptor está a nivel HIGH +++++++++++ //+++++++++++CONTROL DESDE EL MANDO DE LA PS2X++++++++++++ //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if ((digitalRead(Interruptor_PS2X_Joystick)== HIGH)and (digitalRead(Interruptor_VB17)== HIGH)) { if (ps2x.Button(PSB_BLUE)) { digitalWrite(led,HIGH); } if (ps2x.Button(PSB_RED)) { digitalWrite(led,LOW); } //-----------------------------------------------------------------------------------//CON ESTAS INSTRUCCIONES invirtiendo 180,0 /0,180 / o 90,0 / 0,90 //CONSIGO ASIGNAR LOS MISMOS SENTIDOS DE GIRO CON LOS JOYSCTICK DEL MANDO PS2X //que con los de la MESA CONTROLADORA //En los joystick del mando PS2X el //tope hacia el frente valor 0 //tope hacia atrás valor 255 ("No valor 1023") //1 del psp2x //---Lee cualquier entrada de joystick menos la de A0 y A1 que corresponden con las señales //--- PSAB_PAD_UP(11) y PSAB_PAD_DOWN (12)ABRIR CERRAR PINZA M1_A_C //--- Tampoco lee las entradas analogicas del M4 que es A5, que corresponden con PSAB_PAD_LEFT(9)y PSAB_PAD_RIGHT(10)---//--Nota la lectura de las señales de los botonees analógicos deben ser transformadas y adaptadas para activar //--los motores adecuamente //----calculo de A0 y A1 , (subir/bajar) y Rotar pinza,-----//---(Estas instruccionestambien están cuando se maneja por PS2X-----//lectura de A0 para subir Bajar pinza //lee el valor del joystick 1 de la ps2x entrada Analogica A0 q controla Subida-Bajada de LA PINZA valorA0 = ps2x.Analog(PSS_LY); valorA0 = map (valorA0,0,255,0,180); delayMicroseconds(100); // tiempo para la conversion Analogica/digital son 100usg que son 0,1 ms. //lectura desde la PS2X de A1 (M1_I)De Rotar pinza //le el valor del joystick 1 de la ps2x (M1_1)entrada Analogica A1 q controla la ROTACIÓN DE LA PINZA valorA1 = ps2x.Analog(PSS_LX); valorA1 = map (valorA1,0,255,0,180); delayMicroseconds(100); //--------------------------------------------------------------------------------// ----Lee todos los joysticks de la ps2x MENOS el de los Botones Analogicos -----//--------------------------------------------------------------------------------if ((valorPSS != 9) and (valorPSS != 10)and (valorPSS != 11)and (valorPSS != 12)) { //valor analógico del mando ps2x convertido a 0,180 Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

49

servoVal = map(ps2x.Analog(valorPSS),0,255,0,180); delayMicroseconds(100); // tiempo para la conversion Analogica/digital son 100usg //los valores de valorA0 y valorA1 ya estan anteriormente calculados } //fin del if ((valorPSS != 9) ... //-------------------------------------------------------------------------------//--------Cálculo de servoVal en los joystick de los pulsadores analógicos ------//-------------------------------------------------------------------------------//pulsador analogico de la PS2X: 9(PSAB_PAD_LEFT) motor M4 izqda if (valorPSS == PSAB_PAD_LEFT) { servoVal = map(ps2x.Analog(PSAB_PAD_LEFT),0,255,90,0); delayMicroseconds(100); // tiempo para la conversion Analógica/digital son 100usg }

//pulsador analógico de la PS2X: 10 (PSAB_PAD_RIGHT)motor M4 decha. if (valorPSS == PSAB_PAD_RIGHT) { servoVal = map(ps2x.Analog(PSAB_PAD_RIGHT),0,255,90,180); delayMicroseconds(100); // tiempo para la conversion Analogica/digital son 100usg. }

//pulsador analogico de la PS2X: 11(PSAB_PAD_UP) motor M1_A_C Abrir pinza if (valorPSS == PSAB_PAD_UP) { servoVal = map(ps2x.Analog(PSAB_PAD_UP),0,255,90,0); delayMicroseconds(100); // tiempo para la conversion Analogica/digital son 100usg. }

//pulsador analogico de la PS2X: 12 (PSAB_PAD_DOWN) motor M1_A_C Cerrar pinza if (valorPSS == PSAB_PAD_DOWN) { servoVal = map(ps2x.Analog(PSAB_PAD_DOWN),0,255,90,180); delayMicroseconds(100); // tiempo para la conversion Analogica/digital son 100usg } // ----FIN del calculo "servoVal" de los joystick de la ps2x de botonees analógicos---// el calculo de valorA0 (M1_D + M1i) y valorA1 (M1_I) } // fin del if ((digitalRead(Interruptor_PS2X_Joystick)== HIGH)and (digitalRead(Interruptor_VB17)== HIGH))) //-----------------------------------------------------------------------------------------------------------//+++++++++ Si el interruptor está a nivel LOW +++++++++++ //+++CONTROL DESDE los Joystick DE LA MESA CONTROLADORA ++ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if ((digitalRead(Interruptor_PS2X_Joystick)== LOW)and (digitalRead(Interruptor_VB17)== HIGH)) { servoVal = analogRead(pinAnalogico); servoVal = map (servoVal,0,1023,0,180); delayMicroseconds(100); // tiempo para la conversion Analogica/digital son 100usg //--------calculo de A0 y A1 , (subir/bajar) y Rotar pinza,--------Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

50

//(Estas instrucciones también están cuando se maneja por PS2X-----//lectura de A0 para subir Bajar pinza valorA0 = analogRead(A0); //lee el valor del joystick 1 entrada Analogica A0 valorA0 = map (valorA0,0,1023,0,180); delayMicroseconds(100); // tiempo para la conversion Analogica/digital son 100usg //lectura de A1 De Rotar pinza valorA1 = analogRead(A1); //le el valor del joystick 1 entrada Analogica A1 valorA1 = map (valorA1,0,1023,0,180); delayMicroseconds(100); // tiempo para la conversion Analogica/digital son 100usg }//fin if control desde la mesa //---------------------------------------------------------------------------//--------------------------------------------------------------// ++++++CHEQUEAMOS LOS INTERRUPTORES DE GRABAR Y SECUENCIAR+++++ //--------------------------------------------------------------if ((digitalRead (Interruptor_grabar)== HIGH) and digitalRead (Interruptor_secuenciar)== HIGH) { //-------color del Led RGB AMARILLO cuando -------//-----Interruptor_secuenciar)== HIGH --> SecuenciaranalogWrite(rojo,240); analogWrite(verde,245); analogWrite(azul,7); //------------------------------} //-----------------------------------------------// ----- Control Interruptor 7 de GRABACIÓN -----//-----------------------------------------------//pin 7 a cero para grabar movimientos if ((digitalRead(Interruptor_grabar) == LOW)and (FlagGrabar) == 0) { StopMotores(); // para todos los motores, todos llevan 200 ms //lo primero es ir a posición HOME para tomar como referencia.

Mensaje_Retorno_Posicion() ; // a posición Home. //----lo segundo borrar el anterior archivo de secuencias de movimiento--// ------ y crear uno nuevo para las nuevas posiciones --------------SD.remove("posicmot.txt"); miarchivo = SD.open("posicmot.txt", FILE_WRITE); FlagGrabar = 1; //flag de que se está grabando y no repetir función } // Fin del if ((digitalRead(Interruptor_grabar) == LOW).... if (digitalRead(Interruptor_grabar) == LOW) { //al Joystic A0 le corresponde el Potenciometro A6 valorPotX = analogRead (pinAnalogico+6); valorPotX = map (valorPotX,0,1023,0,180);

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

51

Grabar(valorPotX,letra); // ir a grabar y luego mover los motores con joystick } //para que no cierre más el archivo if ((digitalRead (Interruptor_grabar)== HIGH)and (FlagGrabar) == 1) { //hay en él un delay de 200ms por lo que no es necesario el debounce StopMotores(); miarchivo.close(); FlagGrabar = 0; // Iniciamos el flag apagarLeds ();

} //-----------------------------------------------------------------------------//-----------------------------------------------// ----- Control Interruptor 8 de SECUENCIAR ----//-----------------------------------------------//Si pin 8 a cero ir a secuenciar-movimiento if ((digitalRead(Interruptor_secuenciar) == LOW)and (FlagSecuenciar) == 0) { //hay en él un delay de 200ms por lo que no es necesario el debounce StopMotores(); //lo primero es ir a posición HOME para tomar como referencia.

Mensaje_Retorno_Posicion() ; // ir a Home FlagSecuenciar = 1; //flag de que se está secuenciando y no repetir función } // Fin del if (digitalRead((Interruptor_secuenciar) == LOW)and (FlagSecuenciar == 0)) if (digitalRead(Interruptor_secuenciar) == LOW) { Secuenciar_movimientos(); // si interruptor 8 a cero; "Secuenciar_movimientos()" } // fin del if (digitalRead((Interruptor_secuenciar) == LOW)) //para que no cierre más el archivo if ((digitalRead (Interruptor_secuenciar)== HIGH)and (FlagSecuenciar) == 1) { //hay en él un delay de 200ms por lo que no es necesario el debounce StopMotores(); miarchivo.close(); FlagGrabar = 0; // Iniciamos el flag apagarLeds ();

} //--------------------------------------------------------------------// ------------------fin chequeo de interruptores---------------------//--------------------------------------------------------------------//---------------------------------------------------------------//------ INSTRUCCIONES PARA MOVER LOS MOTORES CON LOS JOYSTICK --//---------------------------------------------------------------//Por defecto y como valor global; valorPWM_S = 200 y valorPWM_B = 150. valorPWM_S = 200 ; valorPWM_B = 180; if ((pinAnalogico == 0) or (pinAnalogico == 1)) Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

52

{

valorPWM_S = 150 ; // si es el motor M1_D o M1_I velocidad 150 }

if ((pinAnalogico == 3) or (pinAnalogico == 5)) //el pinAnalogico ==3 corresponde al M2 { valorPWM_S == 255; //Para activar el M2 y M4 a Máxima velocidad valorPWM_B = 200; } //al Joystick A0 le corresponde el Potenciometro A6 valorPotX = analogRead (pinAnalogico+6); valorPotX = map (valorPotX,0,1023,0,180); delayMicroseconds(100);// 100 microsegundos para volver a leer analogRead (...) //-----------------------------------------------// Posición NEUTRA de los JOYSTICK un poco aumentada para //garantizar estabilidad de manejo. He comprobado que oscila //entre 87 a 92 pero por seguridad pongo 80 a 94. // "sin embargo los joystick de la ps2x mantiene un valor muy estable en 90" //1 Posición NEUTRA de los JOYSTICK //Excluye M1_D y M1_I (Analog A0 y A1) if ((servoVal>= 80) and (servoVal <= 94) and (pinAnalogico !=0) and (pinAnalogico !=1)) { // StopMotores(); digitalWrite (int1,LOW); digitalWrite (int2,LOW); digitalWrite (pinPWM,LOW); } //--Para PARAR M1_D + M1_I deben estar; el Joystick 1 (A0) en reposo y El Joystick 1 (A1) también en reposo---//2 para M1_D y M1_I si sus joystick en reposo, pinAnalogico: A0 Y A1 if ((servoVal>= 80) and (servoVal <= 94) and (pinAnalogico == 0) and ((valorA1 >= 80)and (valorA1 <= 94))) { //para que pare los dos motores M1_D y el M1_I // StopMotores(); digitalWrite (22,LOW);//(int1,LOW); digitalWrite (23,LOW);//(int2,LOW); digitalWrite (2,LOW);//(pinPWM,LOW); //---- PONGO TAMBIÉN EL ANULAR LAS SALIDAS DEL M1_I que por defecto no se pondrían -digitalWrite (24,LOW); digitalWrite (25,LOW); digitalWrite (3,LOW); //-----------------------------------------------------------------------------------} //----CREO QUE PUEDO BORRARLO POR NO HABER MEJORA PERO LO DEJO POR VERLO CORRECTO VISTO LO ANTERIOR---if ((servoVal>= 80) and (servoVal <= 94) and (pinAnalogico == 1) and ((valorA0 >= 80)and (valorA0 <= 94))) { //Para que pare los dos motores M1_D y el M1_I // StopMotores(); digitalWrite (22,LOW);//(int1,LOW); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

53

digitalWrite (23,LOW);//(int2,LOW); digitalWrite (2,LOW);//(pinPWM,LOW); //---- PONGO TAMBIÉN EL ANULAR LAS SALIDAS DEL M1_I que por defecto no se pondrían -digitalWrite (24,LOW); digitalWrite (25,LOW); digitalWrite (3,LOW); } //------------------------------------------------------------------------//--------------------------------------------------------//--------------------ACTIVACIÓN MOTORES -----------------//--------------------------------------------------------//--Activa el motor correspondiente salvo los de la pinza M1_D y M1_I---//3 if ((servoVal < 80) and ((pinAnalogico != 0)and (pinAnalogico != 1))) { analogWrite (pinPWM,valorPWM_S); //AQUI M4 GIRA HACIA A MÍ A IZQDA digitalWrite (int1,LOW); digitalWrite (int2,HIGH); //debo poner un delay > a 25 en delay(75); // OBLIGATORIO PONER un delay >25ms lo dejo en 75 pero también muy bien en 200ms // ESTE DELAY PERMITE QUE EL M1_A_C CIERRE PINZA.!!! DE LO CONTRARIO SÓLO ABRIRÍA!!! } //---------------------------------------------------------------------------//3.1 Activa motor, salvo M1_D y M1_I y M4 if ((servoVal > 94)and (pinAnalogico != 0) and (pinAnalogico != 1) and (pinAnalogico!=5)) { //aquí le tengo excluido el M4 CON LO DE !=5

}

analogWrite (pinPWM,valorPWM_B); //velocidad de giro Bajar digitalWrite (int1,HIGH); digitalWrite (int2,LOW); delay(75); // OBLIGATORIO PONER un delay >25ms lo dejo en 75 pero también muy bien en 200ms // ESTE DELAY PERMITE QUE EL M1_A_C ABRA PINZA.!!! // PERO NO ES IMPRESCINDIBLE ESTANDO EL OTRO delay puesto cuando es <80!!!

//M4, MEJOR PONERLO EN EXCLUSIVA CUANDO SE TRATE DEL MOTOR 4 PARA PODER RETROCEDER EN DIRECCIÓN DCHA. //¡¡¡PROBADO Y MUCHO MÁS FLUIDO VA!!! if ((servoVal > 94)and (pinAnalogico == 5)) { analogWrite (pinPWM,valorPWM_S); //aquí en máxima velocidad: valorPWM_S digitalWrite (int1,HIGH); digitalWrite (int2,LOW); delay(75); // OBLIGATORIO PONER un delay >25ms, lo dejo en 75, pero también muy bien en 200ms // girara en sentido a dcha.ESTE DELAY PERMITE QUE EL M4 PUEDA ROTAR EN DIRECCIÓN DCHA.

} //-----------------------------------------------------------------------------------------------//---PARA HACER SUBIR LA PINZA ACTIVANDO M1_D + M1_I-----//4 if ((servoVal < 80) and (pinAnalogico == 0)and ((valorA1 >= 80)and (valorA1 <= 94)))

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

54

{

analogWrite (2,valorPWM_S); //este activa pinPWM 2 digitalWrite (22,HIGH); digitalWrite (23,LOW); analogWrite (3,valorPWM_S); //y este el pinPWM 3 digitalWrite (24,HIGH); digitalWrite (25,LOW);

} //---PARA HACER BAJAR LA PINZA ACTIVANDO M1_D + M1_I-----//4.1 Hace Bajar pinza M1_D + M1_I if ((servoVal > 94) and (pinAnalogico == 0)and ((valorA1 >= 80)and (valorA1 <= 94))) { analogWrite (2,valorPWM_S); //este activa pinPWM 2 digitalWrite (22,LOW); digitalWrite (23,HIGH); analogWrite (3,valorPWM_S); //y este el (pinPWM +1)pinPWM 3 digitalWrite (24,LOW); digitalWrite (25,HIGH);

} //---------------------------------------------------------

//---------------------------------------------------------//---PARA HACER ROTAR LA PINZA ACTIVANDO M1_D + M1_I-----//------------------------Rota a DCHA--------------------//5 if ((servoVal < 80) and (pinAnalogico == 1)and ((valorA0 >= 80)and (valorA0 <= 94))) { analogWrite (2,valorPWM_S); //este activa pinPWM 2 digitalWrite (22,HIGH); digitalWrite (23,LOW); analogWrite (3,valorPWM_S); //y este el pinPWM 3 digitalWrite (24,LOW); digitalWrite (25,HIGH); } //---PARA HACER ROTAR LA PINZA ACTIVANDO M1_D + M1_I-----//------------------------Rota a IZQDA--------------------//5.1 HACE ROTAR PINZA A IZQDA if ((servoVal > 94) and (pinAnalogico == 1)and ((valorA0 >= 80)and (valorA0 <= 94))) {

analogWrite (2,valorPWM_S); //este activa pinPWM 2 digitalWrite (22,LOW); digitalWrite (23,HIGH); analogWrite (3,valorPWM_S); //y este el pinPWM 3 digitalWrite (24,HIGH); digitalWrite (25,LOW);

} //---------------------------------------------------------Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

55

// ---------------------------------------------------------------// -------- TOPES ----//----------------------------------------------------------------servoVal = analogRead (6); // servoVal = map(servoVal,0,1023,0,180); Serial.print("MOTOR 1_D "); Serial.println(servoVal); delayMicroseconds(100); servoVal = analogRead (7); servoVal = map(servoVal,0,1023,0,180); Serial.print("MOTOR 1_I "); Serial.println(servoVal); delayMicroseconds(100);

// ---EVITA TOPES de subida y bajada de la pinza. M1_D + M1_i margen bajada 53º subida 120º--//----- Instrucciones EVITA TOPE de BAJADA M1_D + M1_I------------if ((valorPotX < 55) and ((pinAnalogico == 0)or (pinAnalogico == 1))) { //----Para los motores de la pinza controlados por el pin Analogico A0--digitalWrite (pinPWM,LOW); digitalWrite (int1,LOW); digitalWrite (int2,LOW); //-------ACTIVA " M1_D"EN SENTIDO CONTRARIO para salir del Tope inferior 54º-----analogWrite (pinPWM,valorPWM_S); //este activa pinPWM 2 podría ser el 2 o el 3 digitalWrite (int1,HIGH); digitalWrite (int2,LOW); delay(150);// tiempo para q retroceda } //---------------Fin evita tope de bajada M1_D + M1_I-------------------//----- EVITA TOPE de SUBIDA de la pinza M1_D------------if ((valorPotX > 120) and((pinAnalogico == 0)or (pinAnalogico == 1))) //A0 corresponde a los Motores de pinza M1_D + M1_I { //----Para los motores de la pinza controlados por el pin Analogico A0--digitalWrite (pinPWM,LOW); digitalWrite (int1,LOW); digitalWrite (int2,LOW); //-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope superior 120º---------analogWrite (pinPWM,valorPWM_S); //este activa pinPWM 2 podría ser el 2 o el 3 digitalWrite (int1,LOW); digitalWrite (int2,HIGH); delay(150);// tiempo para q retroceda } //----------Fin de EVITAR TOPES DE SUBIDA y BAJADA de la pinza M1_D + M1_I---------Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

56

//------------------------------TOPES MOTOR 2 M2 -----------------------------// El potenciómetro del M2 NO ES MULTIVUELTAS. Pot2 A9. TOPES: //Inferior 108º superior 2º .Evita podamos deteriorarlo. //----------------------------------------------------------------------------//-------Para evitar bloqueo por llegar a extremos Para M2 - M3 -M4 ----------//----------------------------------------------------------------------if ((valorPotX <= 2) and (pinAnalogico == 3)) //A3 corresponde al M2 del Brazo 1 { //Evita tope extremo inferior de M2 a 108º //------para estabilizar y evitar que dañe potenciometro---digitalWrite (int1,LOW); digitalWrite (int2,LOW); digitalWrite (pinPWM,LOW); //delay(25); //-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope-------analogWrite (pinPWM,valorPWM_S); digitalWrite (int1,HIGH); digitalWrite (int2,LOW); delay(150); //Garatiza que NUNCA LLEGUE A TOPAR "AQUÍ PUESTO, EVITA TOPE" }// Fin if ((valorPotX <108) and .... // evita tope extremo superior del Brazo 1 del M2 en 108º if ((valorPotX >= 108)and (pinAnalogico == 3)) { //------para estabilizar y evitar que dañe potenciometro---digitalWrite (int1,LOW); digitalWrite (int2,LOW); digitalWrite (pinPWM,LOW); //-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope-------analogWrite (pinPWM,valorPWM_S); digitalWrite (int1,LOW); digitalWrite (int2,HIGH); }

delay(150); //Garatiza que NUNCA LLEGUE A TOPAR "AQUÍ PUESTO, EVITA TOPE"

//--------------------------------------------------------------------------//----------------------------TOPES MOTOR 3 M3 ---------------------------------// Evita que el M3 choque con patilla Potenciómetro 3 margen 140º izda a 42º dcha //------------------------------------------------------------------------------//A4 corresponde al M3 del Brazo 2 o columna if ((valorPotX < 42) and (pinAnalogico == 4)) { //----Para el motor M3 Controlado por el pin Analogico A4--digitalWrite (int1,LOW); digitalWrite (int2,LOW); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

57

digitalWrite (pinPWM,LOW); //-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope-----------------

}

analogWrite (pinPWM,valorPWM_S); digitalWrite (int1,HIGH); digitalWrite (int2,LOW); delay(150);

//A4 corresponde al motor M3 brazo columna if ((valorPotX > 140)and (pinAnalogico == 4)) { //----Para el motor M3 Controlado por el pin Analogico A4--digitalWrite (int1,LOW); digitalWrite (int2,LOW); digitalWrite (pinPWM,LOW); //-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope---------------analogWrite (pinPWM,valorPWM_S); digitalWrite (int1,LOW); digitalWrite (int2,HIGH); delay(150); } //----------------------------TOPES MOTOR 4 M4 -----------------------------// Evita que el M4 QUE ES EL QUE HACE GIRAR COLUMNA llegue a tope Dcho: 31º //(realmente puede llegar a 30) //y su tope Izqdo es de 148º aunque puede llegar a 150º //----------------------------------------------------------------------------//A5 corresponde al M4 Motor q hace girar columna. if ((valorPotX < 31) and (pinAnalogico == 5)) { //----Para el motor M4 Controlado por el pin Analogico A5--digitalWrite (int1,LOW); digitalWrite (int2,LOW); digitalWrite (pinPWM,LOW); // delay(25); //espera a q se pare //-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope-------------analogWrite (pinPWM,valorPWM_S); digitalWrite (int1,HIGH); digitalWrite (int2,LOW); delay(150); } if ((valorPotX > 148) and (pinAnalogico == 5)) { //----Para el motor M4 Controlado por el pin Analogico A5--digitalWrite (int1,LOW); digitalWrite (int2,LOW); digitalWrite (pinPWM,LOW); //-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope--------------Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

58

analogWrite (pinPWM,valorPWM_S); digitalWrite (int1,LOW); digitalWrite (int2,HIGH); delay(150); } //----------------------------------------------------------------------------} // fin de la funcion JoystickPS2X (...) //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//-----------FUNCIÓN ir a Posicion HOME --------------------int Home (int PotMotorX,String motor,int pinPWM,int int1,int int2) // ---------------------------------------------------------{ servoVal = analogRead (PotMotorX); delayMicroseconds(100); // tiempo entre lecturas 0,1 ms servoVal = map (servoVal,0,1023,0,180); Serial.print (motor); Serial.print (" "); Serial.print (servoVal); Serial.println (" Grados"); //Por defecto y como valor global; valorPWM_S = 180; //180; valorPWM_B = 180;//150; if (motor == "motor M4") { valorPWM_S = 235; } if (motor == "motor M2") { valorPWM_S = 200; } while (1) { servoVal = analogRead (PotMotorX); delayMicroseconds(100); servoVal = map (servoVal,0,1023,0,180); //-----------------------Serial.print (motor); Serial.print (" "); Serial.print (servoVal); Serial.println (" Grados"); //-----------------------//-------------------Posición HOME 90º----------------if (servoVal== 90) { Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

59

digitalWrite (pinPWM,LOW); digitalWrite (int1,LOW); digitalWrite (int2,LOW); Serial.print("ESTA EN POSICION "); Serial.println (servoVal); delay(150); //de nuevo analizar y actuar en consonancia: servoVal = analogRead (PotMotorX); delayMicroseconds(100); servoVal = map (servoVal,0,1023,0,180); } //--Para activar led RGB en verde, indica que está en posición HOME--if (servoVal== 90) { contaje += 1; if (contaje == 6){ ledVerde(); contaje = 0; } else { ledRojo(); } break; //salir } //---------------------------------------------------------------// -----------------Activar Motores a Home----------------------//--------------------------------------------------------------if (servoVal < 90) { analogWrite (pinPWM,valorPWM_S); digitalWrite (int1,HIGH); digitalWrite (int2,LOW); }

// velocidad giro Bajar LENTA if ((servoVal > 90)and (motor != "motor M4")) { analogWrite (pinPWM,valorPWM_B); digitalWrite (int1,LOW); //(int1,HIGH); digitalWrite (int2,HIGH); // (int2,LOW); }

// velocidad Bajar alta para M4 if ((servoVal > 90)and (motor == "motor M4")) {

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

60

analogWrite (pinPWM,valorPWM_S); digitalWrite (int1,LOW);//(int1,HIGH); digitalWrite (int2,HIGH);// (int2,LOW);// } }//fin del while } // fin de la funcion Home //--------------------------------------------------------//-------Función de Stop Paro de Todos los motores---------//--------------------------------------------------------void StopMotores () { int i = 0; digitalWrite (2,LOW);// PONE A 0 las señales PWM de los enables Drivers digitalWrite (3,LOW); digitalWrite (4,LOW); digitalWrite (5,LOW); digitalWrite (6,LOW); digitalWrite (44,LOW); for (i=0; i<12; i++) { digitalWrite((22+i),LOW); // Pone a cero de la salida 22 a la 33 } delay(200); //tiempo que da para que paren todos } // fin de la funcion void StopMotores //--------------------------------------------------------------------//--------Función con mensaje que va a retornar ------//---------a la posición inical de 90º (HOME)-------------//--------------------------------------------------------------------void Mensaje_Retorno_Posicion() //----------------------------------------------------{ ledRojo(); //led rojo mientras se va a Home, también se mantiene //en la Secuenciar_movimientos() Serial.println ("El brazo ira a posicion inicial 90 grados"); Serial.println ("despues de 2 segundos mover Joystick"); delay (2000); //tiempo para comenzar a utilizar el Joystick //o efectuar el secuenciador /* ------------------------------------------------------motores y pines de los joystick de control + potenciómetros de posición de dichos motores.

-------------- Entradas Analógicas de Control-----------MOTOR PINANALOGICO POTX ACCIÓN Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

61

M1_D A0 A6 Sube/baja pinza (Activa M1_D+ M1_I) M1-I A1 A7 Rota pinza M1_A_C A2 A8 Abre-Cierra Pinza M2 A3 A9 Motor 2 sube-baja M3 A4 A10 Motor 3 sube Baja M4 A5 A11 Motor 4 de columna-base --------------------------------------------------------*/ //------Desplazar los ejes a "Home (.......) -----------///int Home (int PotMotorX,String motor,int pinPWM,int int1,int int2) //letra D Preferible en primer lugar el motor M2 DEL BRAZO DE LA PINZA // se evita que si el M2 esta muy bajo y el giro del 4 es grande //choque la pinza con la base del robot Home (9,"motor M2",4,26,27); // letra D está en primer lugar como seguridad Home (6,"motor M1_D",2,22,23); //Motor M1_D Home (7,"motor M1_I",3,24,25); //Motor M1_I

letra A letra B

Home (8,"motor M1_A_C",6,30,31); //Motor M1_A_C letra C Home (10,"motor M3",5,28,29); //Motor M3

letra E

Home (11,"motor M4",44,32,33); //Motor M4

letra F

//------------------------------------------------------} // fin void Mensaje_Retorno_Posicion() //---------------------------------------------------------------------------------------------//------FUNCIÓN para GRABAR EN SD las posicones de los motores ------//---------------------------------------------------------------------------------------------int Grabar(int valor,char letra) {

if (miarchivo != 0) // si se abre; grabar { ledVerde(); //activar Led Verde //NO UTILIZO miarchivo.write(servoVal); porque los enteros se ven //escritos en la SD como signos raros. Y no me gusta //aun siendo más fácil su uso miarchivo.print(valor); //valor_a_grabar. El nº 123 se escribe como '1' '2' '3' miarchivo.print(letra); //el 13 y el 10 retorno y cambio linea finalizan la copia de cada posición miarchivo.println(); Serial.print(" VALOR SD; "); Serial.print(valor);// (valor_a_grabar); Serial.println(letra); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

62

}//fin del if (miarchivo != 0) }//----------- Fin funcion Grabar-------------//----------------------------------------------------------------------------------//--------------FUNCIÓN para secuenciar los movimientos----------//----------------------------------------------------------------------------------void Secuenciar_movimientos() { //Ya estaban declaradas como globales String valorString = ""; //Valor para convertir mas tarde a decimal entero int valorInt = 0 ; //para iniciar su valor NO BORRAR char c ; //nombre del archivo con los pasos grabados miarchivo = SD.open("posicmot.txt"); // si se abre OK leer todas las posiciones grabadas en la SD if (miarchivo != 0) { ledAzul(); //activar el led azul en Secuenciar_movimientos() while (miarchivo.available() > 0) { //Lee las posiciones de los motores grabadas en la SD // en la SD se guarda el ENTERO como ASCII con la instruccion miarchivo.print(servoVal). //El valor ASCII de la SD lo paso a int servoVal //para ver cuando se pulsa enter 13D o nueva linea 10D servoVal = (miarchivo.read()); c = char(servoVal); // pasa a carácter el valor decimal // también son decimales el 13-10 enter y nueva linea //que es lo que separa cada posción guardada en SD //NO CONCATENAR dichos valores //---------------------------------------------------if ((servoVal!=13)and (servoVal!=10))// { valorString.concat(c); //"*.toInt()pasa una cadena de caracteres a un numero entero // ejemplo 123abc23 lo transforma en el entero 123 valorInt = valorString.toInt(); }// fin del if ((servoVal!=13)and (servoVal!=10)) //---------------------------------------------------//Al llegar al cambio de línea, activar el motor Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

63

if (c == 'A') //Cuando sea cambio linea { Serial.print("Secuenciar MOTOR M1_D "); Serial.print(valorInt); Serial.println('A'); //int ActivarMotores ( int potMotorX,int valorInt,int pinPWM,int int1, int int2) ActivarMotores(6,valorInt,2,22,23); // para activar el Motor M1_D valorString = "" ; //iniciar la variable. // NOTA no es necesario reiniciar "valorInt" al haber reiniciado: valorString = "" ; } //fin del if (c == 'A') if (c == 'B') //Cuando sea cambio linea { Serial.print("Secuenciar MOTOR M1_I "); Serial.print(valorInt); Serial.println('B'); ActivarMotores(7,valorInt,3,24,25); // para activar el Motor M1_I valorString = "" ; } //fin del if (c == 'B') if (c == 'C') //Cuando sea cambio linea { Serial.print("Secuenciar MOTOR M1_A_C "); Serial.print(valorInt); Serial.println('C'); ActivarMotores(8,valorInt,6,30,31); // para activar el Motor M1_A_C valorString = "" ; } //fin del if (c == 'C') if (c == 'D') //Cuando sea cambio linea { Serial.print("Secuenciar MOTOR M2 "); Serial.print(valorInt); Serial.println('D'); ActivarMotores(9,valorInt,4,26,27); // para activar el Motor M2 valorString = "" ; } //fin del if (c == 'D') if (c == 'E') //Cuando sea cambio linea Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

64

{ Serial.print("Secuenciar MOTOR M3 "); Serial.print(valorInt); Serial.println('E'); ActivarMotores(10,valorInt,5,28,29); // para activar el Motor M3 valorString = "" ; } //fin del if (c == 'E') if (c == 'F') //Cuando sea cambio linea { Serial.print("Secuenciar MOTOR M4 "); Serial.print(valorInt); Serial.println('F'); ActivarMotores(11,valorInt,44,32,33); //para activar el Motor 4 que hace girar la base valorString = "" ; } //fin del if (c == 'F') if (digitalRead(Interruptor_secuenciar) == HIGH) { FlagSecuenciar = 0; //reiniciamos el flag. break; //salir del while. }/ /FIN del if (digitalRead(Interruptor_secuenciar) == HIGH) } //fin del While miarchivo.available... // Cuando se hayan leído todos los datos del archivo cerrarlo //y salir de la función secuenciar_movimientos StopMotores(); miarchivo.close();//cierra el archivo }//fin del if (miarchivo != 0) } //fin del void Secuenciar_movimientos(); //--------------------------------------------------------------------------------// ----FUNCIÓN "ActivarMotores" para activar los motores ---//------- desde la función void Secuenciar_movimientos()------//--------------------------------------------------------------------------------int ActivarMotores (int PotMotorX,int valorInt,int pinPWM,int int1,int int2)

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

65

{

//Por defecto y como valor global; valorPWM_S = 225 y valorPWM_B = 200 //por valor global para todos, menos para MI_D, M1_I, M1_A_C int valorPWM = 200; valorPWM_S = 225; valorPWM_B = 200; if (PotMotorX == 11) { //se trata del potenciómetro del motor M4 máx velocidad valorPWM_S = 255; valorPWM_B = 255; } //lo utilizo para que no se repitan ciclos innecesarios int valorAnterior = 0 ;

while (1) { //------------------------------------------------------------//1º se leen la posición del potenciometro de los motores //para comparar después con el que debe tener //------------------------------------------------------------servoVal = analogRead (PotMotorX); delayMicroseconds(100); servoVal = map (servoVal,0,1023,0,180); if (servoVal != valorAnterior) //para evitar repetir ciclos { //-----------------------Serial.print ("motor: "); Serial.print (PotMotorX); //motor 0 a 4 Serial.print ("--"); Serial.print (servoVal); Serial.println (" Grados"); //-----------------------//--Con la función "abs" obtengo valor global obviando el signo +/// Para dar una histéresis de +-1º if (abs(servoVal - valorInt) < 2) { digitalWrite (pinPWM,LOW); digitalWrite (int1,LOW); digitalWrite (int2,LOW); Serial.print("ESTA EN POSICION "); Serial.println (servoVal); // De nuevo analizar y actuar en consonancia: si está +-1º respecto al valor // que debe tener salir, de lo contrario continuar servoVal = analogRead (PotMotorX); delayMicroseconds(100); servoVal = map (servoVal,0,1023,0,180); }

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

66

if (abs(servoVal - valorInt) < 2) {

digitalWrite (pinPWM,LOW); digitalWrite (int1,LOW); digitalWrite (int2,LOW);

break; //salir } //---fin de la comparación de ver si está en posición leída---//--------------- SI ES MENOR LA SEPARACION A 3º----- //-------El A9 corresponde al M2 y el A6 y A7 a M1_D y M1_I----if (servoVal < (valorInt-3)) {

analogWrite (pinPWM,valorPWM_S); digitalWrite (int1,HIGH); //LOW digitalWrite (int2,LOW); //HIGH if ((PotMotorX == 6)or(PotMotorX == 7)) { delay(250); //un retraso de 250ms para que no se frenen M1_D con M1_I }

} //HE SUPRIMIDO EL REDUCIR VELOCIDAD DE M3 AL SUBIR POR NO SER NECESARIO Y SI AL BAJAR if (servoVal >= (valorInt-3)and (servoVal< valorInt)) { analogWrite (pinPWM,valorPWM_B); digitalWrite (int1,HIGH); //LOW digitalWrite (int2,LOW); //HIGH if ((PotMotorX == 6)or(PotMotorX == 7)) { delay(250); //un retraso de 250ms para que no se frenen M1_D con M1_I } } //--------------- SI ES MAYOR LA SEPARACION A +3-----Esta corresponden a valorPWM_B--if (servoVal >= (valorInt+3)) { analogWrite (pinPWM,valorPWM_B); digitalWrite (int1,LOW);//HIGH digitalWrite (int2,HIGH);//LOW if ((PotMotorX == 6)or(PotMotorX == 7)) { delay(250); //un retraso de 50ms para que no se frenen M1_D con M1_I } } //PARA REDUCIR VELOCIDAD AL M3 AL BAJAR if (servoVal <= (valorInt+3)and (servoVal >= (valorInt)and(PotMotorX == 10))) { analogWrite (pinPWM,valorPWM_B); digitalWrite (int1,LOW);//HIGH Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

67

digitalWrite (int2,HIGH);//LOW } if (servoVal <= (valorInt+3)and (servoVal >= (valorInt))) { analogWrite (pinPWM,valorPWM_B); digitalWrite (int1,LOW);//HIGH digitalWrite (int2,HIGH);//LOW if ((PotMotorX == 6)or(PotMotorX == 7)) { delay(250); //un retraso de 250ms para que no se frenen M1_D con M1_I } } } // Fin del if (servoVal != valorAnterior) valorAnterior = servoVal; }//fin del while } // fin de la funcion Activar movimientos

//******************************************************** // ****MÁS FUNCIONES, ESTAS PARA MANEJO DESDE VB2017 ***** //******************************************************** //------------------------------------------------//----------FUNCION READSTRING()-----//------------------------------------------------//Lee trama de configuración de puerto serie y la segmenta en instrucciones //que se almacenarán en el vector de trayectorias //Utiliza punteros para simular una función split que separe las tramas //por el caracter de control. void readString() { char c; //**--ya la tengo como global int buffSize = 0; int buc=0; char *p; char *str; char *str2; char *p2; char *str3; char *p3; int postmp=0; int subpostmp=0; int posmotor=2; int countbucle; int total_chars=0; int verifica_chars=0; int filas=0; Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

68

int columnas=0; //vaciamos los arrays memset(buffer, 0, sizeof(buffer)); for(filas=0;filas<INSTRUCCIONES;filas++){ for(columnas=0;columnas<7;columnas++){ pos[filas][columnas]=0; } } Serial.flush(); do{ while (Serial.available()>0){ buffer[buffSize]=Serial.read(); if(buffer[buffSize]=='#'){control_prog=0;break;} buffSize++; } }while(control_prog==1); p=buffer; while ((str = strtok_r(p, "$", &p)) != NULL){ //cadena en str if(postmp==0){total_progs=atoi(str);} else if(postmp>0 && postmp<=total_progs){ p2=str; subpostmp=0; countbucle=0; while ((str2 = strtok_r(p2, "!", &p2)) != NULL){ if(subpostmp==0){subpostmp=atoi(str2);} else{ if(countbucle==1){ if(str2[0]=='M'){ pos[subpostmp-1][0]=1; }else if(str2[0]=='H'){ pos[subpostmp-1][0]=2; } }else if(countbucle==2){ if(str2[0]=='C'){ pos[subpostmp-1][1]=1; }else if(str2[0]=='A'){ pos[subpostmp-1][1]=2; }else if(str2[0]=='I'){ pos[subpostmp-1][1]=3; }else if(str2[0]=='L'){ pos[subpostmp-1][1]=4; }else if(str2[0]=='T'){ pos[subpostmp-1][1]=5; } }else if(countbucle==3){ if(str2!=""){ p3=str2; posmotor=2; while ((str3 = strtok_r(p3, ";", &p3)) !=NULL){ pos[subpostmp-1][posmotor]=atoi(str3); posmotor++; }//fin while str3 }//fin if str2="" }//fin countbucle==3 }//fin subpostmp==1 Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

69

countbucle++; }//fin while str2 }//fin else postmp>0 y <total_progs else{ total_chars=atoi(str); } postmp++; }//fin while str //para comprobar si la transmisión fue correcta tenemos que quitar //los caracteres del contador total y el de fin de trama

}

if(total_chars<10){ verifica_chars=buffSize-5; }else if (total_chars>=10 && total_chars<100){ verifica_chars=buffSize-3; }else if (total_chars>=100){verifica_chars=buffSize-4;} if(verifica_chars==total_chars){ //transmision-OK Serial.print("Y-OK"); }else{ //transmision-NOOK Serial.print("Y-NOOK"); } control_prog=0;

//--------------------------------------------------------------//---------FUNCIÓN PARADA_EMERGENCIA()------//--------------------------------------------------------------//Si se acciona el botón parada de emergencia en la aplicación de control, //se pararán todos los motores y el controlador pasa a modo manual void parada_emergencia() { Serial.print("PARADA EMERGENCIA"); control_modo=0; //para activar el modo Manual digitalWrite(ENA_1, HIGH); //pin PWM de M1_D digitalWrite(ENA_2, HIGH); //pin PWM de M1_I digitalWrite(ENA_3, HIGH); //pin PWM de M2 digitalWrite(ENA_4, HIGH); //pin PWM de M3 digitalWrite(ENA_5, HIGH); //pin PWM de M1_A_C digitalWrite(ENA_6, HIGH); //pin PWM de M4 digitalWrite(IN1_1, LOW); digitalWrite(IN2_1, LOW); digitalWrite(IN1_2, LOW); digitalWrite(IN2_2, LOW); digitalWrite(IN1_3, LOW); digitalWrite(IN2_3, LOW); digitalWrite(IN1_4, LOW); digitalWrite(IN2_4, LOW); digitalWrite(IN1_5, LOW); digitalWrite(IN2_5, LOW); digitalWrite(IN1_6, LOW); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

70

}

digitalWrite(IN2_6, LOW); control=1;

//-------------------------------------------------//-------------FUNCIÓN PINZA ()------------//-------------------------------------------------//Abre o cierra la mano según el parametro de control controlpinza //Esta función llama a la función gira3, encargada del movimiento de la misma void pinza(int controlpinza) //si 1 cerrar, si 0 abrir { int c1=0; if (controlpinza == 1){ Setpoint3= M3MIN;// Tiene este valor 634 cerrada pinza. los topes de posicion en 1 es Cerrado y 0 es Abierto sin dañar Motor1 }else{ Setpoint3= M3MAX;//Tiene este valor 400 Abierta pinza } if(Setpoint3>Setpoint_old3){ myPID3.SetControllerDirection(DIRECT); }else{ myPID3.SetControllerDirection(REVERSE); } if((analogRead(sensorPin3)>(Setpoint3+5))||(analogRead(sensorPin3)<(Setpoint3-5))){ gira3(sensorPin3, IN1_5, IN2_5, 100,ENA_5, Setpoint3,myPID3); c1=0; }else{ Setpoint_old3=analogRead(sensorPin3); //es el MOTOR M1_A_C digitalWrite(IN1_5,LOW); digitalWrite(IN2_5,LOW); analogWrite(ENA_5,0); c1=1; } if(c1==1){ control=control+1; } } //----------------------------------------------------//------------FUNCIÓN POSICIÓN-------------//----------------------------------------------------//Esta funcion posiciona el robot según los parametros escogidos y //almacenados en el vector de trayectorias. //Según el parámetro; control_trayectoria, determina la prioridad de //movimiento sobre el plano XY, Z o indiferente void posicion (int control_trayectoria){ int c1=0; int c2=0; int c3=0; int c4=0; int c5=0; int c6=0; int control_prioridad; Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

71

if(control_trayectoria==1){ control_prioridad=1; }else if(control_trayectoria==2){ control_prioridad=0; }else{ control_prioridad=2; } do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial =='P'){ parada_emergencia(); break; break; } if(control_prioridad==0 || control_prioridad==2){ /* // --- Incluyo al Motor 1 de la mano ----if(Setpoint1>Setpoint_old1){ myPID1.SetControllerDirection(REVERSE); }else{ myPID1.SetControllerDirection(DIRECT); } */ //----M1_D----if((analogRead(sensorPin1)>(Setpoint1+5))||(analogRead(sensorPin1)<(Setpoint1-5))){ gira1(sensorPin1, IN1_1, IN2_1, 80,ENA_1, Setpoint1,myPID1); c1=0; }else{ Setpoint_old1=analogRead(sensorPin1); digitalWrite(IN1_1,LOW); digitalWrite(IN2_1,LOW); analogWrite(ENA_1,0); c1=1; } //-------------------------------------//----M1_I----if((analogRead(sensorPin2)>(Setpoint2+5))||(analogRead(sensorPin2)<(Setpoint2-5))){ gira2(sensorPin2, IN1_2, IN2_2, 100,ENA_2, Setpoint2,myPID2); c2=0; }else{ Setpoint_old2=analogRead(sensorPin2); digitalWrite(IN1_2,LOW); digitalWrite(IN2_2,LOW); analogWrite(ENA_2,0); c2=1; } //------------------------------------//----M1_A_C----if((analogRead(sensorPin3)>(Setpoint3+5))||(analogRead(sensorPin3)<(Setpoint3-5))){ gira3(sensorPin3, IN1_5, IN2_5, 100,ENA_5, Setpoint3,myPID3); c3=0; }else{ Setpoint_old3=analogRead(sensorPin3); digitalWrite(IN1_5,LOW); Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

72

digitalWrite(IN2_5,LOW); analogWrite(ENA_5,0); c3=1; } //----M2----if((analogRead(sensorPin4)>(Setpoint4+5))||(analogRead(sensorPin4)<(Setpoint4-5))){ gira4(sensorPin4, IN1_3, IN2_3, 100,ENA_3, Setpoint4,myPID4); c4=0; }else{ Setpoint_old4 = analogRead(sensorPin4); digitalWrite(IN1_3,LOW); digitalWrite(IN2_3,LOW); analogWrite(ENA_3,0); c4=1; } //he aĂąadido c1==1 para poder incluir el posicionamiento del MOTOR 1 if(c1==1 && c2==1 && c3==1 && c4==1){ control_prioridad=2; } }//fin control prioridad==0 if(control_prioridad==1 || control_prioridad==2){ //----M3----if((analogRead(sensorPin5)>(Setpoint5+5))||(analogRead(sensorPin5)<(Setpoint5-5))){ gira5(sensorPin5, IN1_4, IN2_4, 100,ENA_4, Setpoint5,myPID5); c5=0; } else{ Setpoint_old5=analogRead(sensorPin5); digitalWrite(IN1_4,LOW); digitalWrite(IN2_4,LOW); analogWrite(ENA_4,0); c5=1; } if(c5==1){ control_prioridad=2; } // //fin control prioridad==1 o 2 //----M4----if((analogRead(sensorPin6)>(Setpoint6+5))||(analogRead(sensorPin6)<(Setpoint6-5))){ gira6(sensorPin6, IN1_6, IN2_6, 100,ENA_6, Setpoint6,myPID6); c6=0; } else{ Setpoint_old6=analogRead(sensorPin6); digitalWrite(IN1_6,LOW); digitalWrite(IN2_6,LOW); analogWrite(ENA_6,0); c6=1; } if(c6==1){ control_prioridad=2; }

Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

73

}//fin control prioridad==1 o 2 if(c1==1 && c2==1 && c3==1 && c4==1 && c5==1 && c6==1){ control=control+1; } }while(c1!=1 || c2!=1 || c3!=1 || c4!=1 || c5!=1 || c6!=1); //añado c1!=1 y || c6!=1); } // Fin de void posicion (int control_trayectoria) //------------------------------------------------//------------FUNCION GIRA1--------------//------------------------------------------------//-------------------------------------------------------------//MOTOR M1_D control desde VB17 (680 /340) //-------------------------------------------------------------//-----------------M1_D------------------// Activa según el posicionamiento deseado void gira1(int id_sensor, int motor_dcha, int motor_izq, int reduccion, int id_enable, double Setpoint, PID myPID){ Input1 = analogRead(id_sensor); double gap = abs(Setpoint-Input1); //distancia entre el punto actual y el definitivo if(Setpoint>Input1){ digitalWrite(motor_dcha,HIGH); digitalWrite(motor_izq,LOW); }else{ digitalWrite(motor_dcha,LOW); digitalWrite(motor_izq,HIGH); } if(gap<reduccion){ //si estamos cerca del punto, reducimos parametro PID myPID.SetTunings(consKp1, consKi1, consKd1); }else{ //si estamos alejados, utilizamos parametros mas agresivos para PID myPID.SetTunings(aggKp1, aggKi1, aggKd1); } myPID.Compute(); var=analogRead(id_sensor); if(var!=var_old){ var2=Output1; var_old=var; } analogWrite(id_enable,Output1); } //------------------------------------------------//------------FUNCION GIRA2--------------//------------------------------------------------//MOTOR M1_I control desde VB17 (680 /340) //-----------------------------------------//Activa según la posición deseada

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

74

void gira2(int id_sensor, int motor_dcha, int motor_izq, int reduccion, int id_enable, double Setpoint, PID myPID){ Input2 = analogRead(id_sensor); double gap = abs(Setpoint-Input2); //distancia entre el punto actualy el definitivo if(Setpoint>Input2){ digitalWrite(motor_dcha,HIGH); digitalWrite(motor_izq,LOW); }else{ digitalWrite(motor_dcha,LOW); digitalWrite(motor_izq,HIGH); } if(gap<reduccion){ //si estamos cerca del punto, reducimos parametro PID myPID.SetTunings(consKp2, consKi2, consKd2); }else{ //si estamos alejados, utilizamos parametros mas agresivos para PID myPID.SetTunings(aggKp2, aggKi2, aggKd2); } myPID.Compute(); var=analogRead(id_sensor); if(var!=var_old){ var2=Output2; var_old=var; } analogWrite(id_enable,Output2); } //-----------------------------------------------//------------FUNCION GIRA3-------------//-----------------------------------------------//----------------------------------------------//MOTOR M1_A_C control desde VB17 (Abierta 400 cerrada 634) //----------------------------------------------//Activa según la posición deseada void gira3(int id_sensor, int motor_dcha, int motor_izq, int reduccion, int id_enable, double Setpoint, PID myPID) { Input3 = analogRead(id_sensor); double gap = abs(Setpoint-Input3); //distancia entre el punto actual y el definitivo if(Setpoint>Input3){ digitalWrite(motor_dcha,HIGH); digitalWrite(motor_izq,LOW); }else{ digitalWrite(motor_dcha,LOW); digitalWrite(motor_izq,HIGH); } if(gap<reduccion){ //si estamos cerca del punto, reducimos parametro PID myPID.SetTunings(consKp3, consKi3, consKd3); }else{ //si estamos alejados, utilizamos parametros mas agresivos para PID myPID.SetTunings(aggKp3, aggKi3, aggKd3); } myPID.Compute(); var=analogRead(id_sensor); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

75

if(var!=var_old){ var2=Output3; var_old=var; } analogWrite(id_enable,Output3); } //-----------------------------------------------//------------FUNCION GIRA4--------------//------------------------------------------------//MOTOR M2 control desde VB17 (arriba 33 abajo 611) //----------------------------------------------//Esta es la función encargada de activar el motor 2 según la posición deseada void gira4(int id_sensor, int motor_dcha, int motor_izq, int reduccion, int id_enable, double Setpoint, PID myPID) { Input4 = analogRead(id_sensor); double gap = abs(Setpoint-Input4); //distancia entre el punto actual y el definitivo if(Setpoint>Input4){ digitalWrite(motor_dcha,HIGH); digitalWrite(motor_izq,LOW); }else{ digitalWrite(motor_dcha,LOW); digitalWrite(motor_izq,HIGH); } if(gap<reduccion){ //si estamos cerca del punto, reducimos parametro PID myPID.SetTunings(consKp4, consKi4, consKd4); }else{ //si estamos alejados, utilizamos parametros mas agresivos para PID myPID.SetTunings(aggKp4, aggKi4, aggKd4); } myPID.Compute(); var=analogRead(id_sensor); if(var!=var_old){ var2=Output4; var_old=var; } analogWrite(id_enable,Output4); } //-----------------------------------------------------//----------------FUNCION GIRA5---------------//-----------------------------------------------------//MOTOR M3 control desde VB17 (arriba 796 abajo 242) //----------------------------------------------//Esta es la función encargada de activar el motor 3 según la posición deseada void gira5(int id_sensor, int motor_dcha, int motor_izq, int reduccion, int id_enable, double Setpoint, PID myPID) { Input5 = analogRead(id_sensor); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

76

double gap = abs(Setpoint-Input5); //distancia entre el punto actual y el definitivo if(Setpoint>Input5){ digitalWrite(motor_dcha,HIGH); digitalWrite(motor_izq,LOW); }else{ digitalWrite(motor_dcha,LOW); digitalWrite(motor_izq,HIGH);

}

} if(gap<reduccion){ //si estamos cerca del punto, reducimos parametro PID myPID.SetTunings(consKp5, consKi5, consKd5); }else{ //si estamos alejados, utilizamos parametros mas agresivos para PID myPID.SetTunings(aggKp5, aggKi5, aggKd5); } myPID.Compute(); var=analogRead(id_sensor); if(var!=var_old){ var2=Output5; var_old=var; } analogWrite(id_enable,Output5);

//-------------------------------------------------//------------FUNCION GIRA6---------------//-------------------------------------------------//MOTOR M4 control desde VB17 ( a dcha 842 a izqda 180) //----------------------------------------------//Esta es la función encargada de activar el motor 4 según la posición deseada void gira6(int id_sensor, int motor_dcha, int motor_izq, int reduccion, int id_enable, double Setpoint, PID myPID) { Input6 = analogRead(id_sensor); double gap = abs(Setpoint-Input6); //distancia entre el punto actual y el definitivo if(Setpoint>Input6){ digitalWrite(motor_dcha,HIGH); digitalWrite(motor_izq,LOW); }else{ digitalWrite(motor_dcha,LOW); digitalWrite(motor_izq,HIGH); } if(gap<reduccion){ //si estamos cerca del punto, reducimos parametro PID myPID.SetTunings(consKp6, consKi6, consKd6); }else{ //si estamos alejados, utilizamos parametros mas agresivos para PID myPID.SetTunings(aggKp6, aggKi6, aggKd6); } myPID.Compute(); var=analogRead(id_sensor); if(var!=var_old){ var2=Output6; var_old=var; } Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

77

}

analogWrite(id_enable,Output6);

//*****FIN FUNCIONES PARA EL MANEJO DESDE VB2017****** //------------------------------void setup() { //--------------------------------// pines 7, 8, 12 y 13 como entradas (el 13 prioridad desde VB2017. En el otro proyecto era el pin 19) pinMode (Interruptor_grabar,INPUT); // Pin 7 como entrada nivel 0 para grabar movimiento pinMode (Interruptor_secuenciar,INPUT); // Pin 8 como entrada nivel 0 para secuenciar //el movimiento grabado pinMode (Interruptor_PS2X_Joystick,INPUT); // Pin 12, nivel (0) manejo del brazo con JOYSTICK de la // mesa de control. A nivel (1) control desde mando PlayStatión PS2X pinMode (Interruptor_VB17,INPUT); // Pin 19 como entrada, nivel (0) manejo con VB17 pinMode (pulsador_joystick4,INPUT_PULLUP); //pin 38 configurado como R interna, se evita R externa. // pines como salidas pinMode(rojo,OUTPUT); //LED rojo pin 9 salida pinMode(verde,OUTPUT); //LED verde pin 10 salida pinMode(azul,OUTPUT); //LED azul pin 11 salida pinMode (led_verde_bajo,OUTPUT); //pin 39 es el led bajo el interruptor de VB17. No me hace falta... pinMode (activar_led_uso_ps2x,OUTPUT);//pin 40 //---------DRIVER 1 L298 ---------------------------// Control L298N del Motor M1_D pinMode(ENA_1, OUTPUT); //pin 2 señal PWM pinMode(IN1_1, OUTPUT); //pin 22 pinMode(IN2_1, OUTPUT); //pin 23 // Control L298N del Motor M1_I pinMode(ENA_2, OUTPUT); //pin 3 señal PWM pinMode(IN1_2, OUTPUT); //pin 24 pinMode(IN2_2, OUTPUT); //pin 25 //---------DRIVER 2 L298 ---------------------------// Control L298N del Motor M2 pinMode(ENA_3, OUTPUT); //pin 4 señal PWM pinMode(IN1_3, OUTPUT); //pin 26 pinMode(IN2_3, OUTPUT); //pin 27 // Control L298N del Motor M3 pinMode(ENA_4, OUTPUT); //pin 5 señal PWM pinMode(IN1_4, OUTPUT); //pin 28 pinMode(IN2_4, OUTPUT); //pin 29 //---------DRIVER 3 L298 ---------------------------// Control L298N del Motor M1_A_C pinMode(ENA_5, OUTPUT); //pin 6 señal PWM pinMode(IN1_5, OUTPUT); //pin 30 pinMode(IN2_5, OUTPUT); //pin 31 // Control L298N del Motor M4 Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

78

pinMode(ENA_6, OUTPUT); //pin 44 señal PWM pinMode(IN1_6, OUTPUT); //pin 32 pinMode(IN2_6, OUTPUT); //pin 33 //-------------------------------------------------// --Para el led como salida---pinMode (led, OUTPUT); // pin 43 como salida para led iluminador del brazo. //Uso el 43 por no tener en el pin 18 salida mi placa shield por mala soldadura seguramente //pin 38 como entrada configurado en PULL-UP, permite eliminar resistencia externa pinMode (pulsador_joystick4,INPUT_PULLUP); digitalWrite (led, LOW); // lo inicio en nivel bajo. Se puede activar desde //mando de la play tecla azul (X) y desactivar con tecla roja (0) //----------------------------//inicio con todos apagados apagarLeds(); //nota tengo en el pin 9 el rojo 10 el verde //y 11 el azul y en el 43 el "led" de iluminación, y que no he podido conectar //al pin 18 por no tener salida en mi shield por mala soldadura //-- Estos se activan por MASA (a nivel 0)--digitalWrite (led_verde_bajo,HIGH); //pin 39 es el led bajo el interruptor de VB17. No me hace falta ... digitalWrite (activar_led_uso_ps2x,HIGH);//pin 40 //--------------------------// -----POSICION INICIAL MOTORES DESCONECTADOS------digitalWrite (ENA_1,LOW); //Desconectar motor M1_D digitalWrite (IN1_1,LOW); digitalWrite (IN2_1,LOW); digitalWrite (ENA_2,LOW) ; //Desconectar motor M1_I digitalWrite (IN1_2,LOW); digitalWrite (IN2_2,LOW); digitalWrite (ENA_3,LOW) ; //Desconectar motor M2 digitalWrite (IN1_3,LOW); digitalWrite (IN2_3,LOW); digitalWrite (ENA_4,LOW) ; //Desconectar motor M3 digitalWrite (IN1_4,LOW); digitalWrite (IN2_4,LOW); digitalWrite (ENA_5,LOW) ; //Desconectar motor M1_A_C digitalWrite (IN1_5,LOW); digitalWrite (IN2_5,LOW); digitalWrite (ENA_6,LOW) ; //Desconectar motor M4 digitalWrite (IN1_6,LOW); digitalWrite (IN2_6,LOW); //---------------------------------------------------//Inicializamos la conexión serial Serial.begin(57600); //**----INICIACIÓN DE LOS SETPOINT DEL PID------- VER PAG 121-122-123

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

79

Setpoint_old1=analogRead(sensorPin1); Setpoint_old2=analogRead(sensorPin2); Setpoint_old3=analogRead(sensorPin3); Setpoint_old4=analogRead(sensorPin4); Setpoint_old5=analogRead(sensorPin5); Setpoint_old6=analogRead(sensorPin6);

//potenciómetro de M1_D. //potenciómetro de M1_I. //potenciómetro de M1_A_C. //potenciómetro de M2. //potenciómetro de M3. //potenciómetro de M4

myPID1.SetMode(AUTOMATIC); myPID2.SetMode(AUTOMATIC); myPID3.SetMode(AUTOMATIC); myPID4.SetMode(AUTOMATIC); myPID5.SetMode(AUTOMATIC); myPID6.SetMode(AUTOMATIC); //Especificación de las limitaciones del brazo robot //para su control de velocidad mínima y máxima myPID1.SetOutputLimits(150, 200); //potencia para M1_D myPID2.SetOutputLimits(150, 200); //potencia para M1_I myPID3.SetOutputLimits(150, 200); //potencia para M1_A_C myPID4.SetOutputLimits(150, 200); //potencia para M2 myPID5.SetOutputLimits(150, 200); //potencia para M3 myPID6.SetOutputLimits(150, 255); //potencia para M4 //----------------------------------------------//----------------- MÓDULO RECEPTOR ACTIVO ??? ----------------//-----------------------------------------------------------// En mi ps2x chino debo hacer el ps2x.config_gamepad(…) //doblemente para que sea reconocido el módulo receptor ps2x //------------------------------------------------------------//error = ps2x.config_gamepad(14,15,16,17, true, true); //ESTOS PINES DE MI PLACA SHIELD NO TIENEN SALIDA POR FALLO EN SOLDADURA // en su lugar uso los pines (34,35,36,37) //NOTA el último pin de config_gamepad (x,x,x,y) //la librería la debe configurar como entrada, he hecho pruebas. error = ps2x.config_gamepad(34,35,36,37, true, true); delay(50); error = ps2x.config_gamepad(34,35,36,37, true, true); delay(50); if(error == 0){ Serial.println("Controlador PS2X, configurado EXITOSAMENTE"); } else if(error == 1) Serial.println("Controlador PS2X NO encontrado"); else if(error == 2) Serial.println("Controlador PS2X encontrado, pero no acepta comandos"); else if(error == 3)

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

80

Serial.println("Controller refusing to enter Pressures mode, may not support it. "); type = ps2x.readType(); switch(type) { case 0: Serial.println("Unknown Controller type"); break; case 1: Serial.println("DualShock Controller Found"); break; } //--------------------------------------------------------//+++++ LECTURA DE LA TARJETA SI NO ESTÁ INSERTADA SE QUEDA A LA ESPERA +++++++++++++ //pinMode(53,OUTPUT);//Necesario para la librería SD(en la MEGA el 53 y en la UNO el 10 //ponerlo como salida aunque no se utilice para compatibilizarlo con la UNO //HE PROBADO QUE INCLUSO SIN PONERLOS VA OK. pinMode(53,OUTPUT);//Necesario para la librería SD en Arduino Mega 2560 // ******LA LECTURA DE LA TARJETA*********************************** Serial.print("Inicializando SD card..."); // ---SE QUEDA A LA ESPERA DE TENER BIEN LA TARJETA.--//el pin cs en la mega es el 53 y en la uno el 10 en la ethernet el 4 if (SD.begin(53)== 0) { Serial.println ("Error de lectura en SD o bien; no hay Tarjeta"); while (SD.begin(53)== 0)//cuando sea 1 salir { //Buble infinito hasta que se inserte tarjeta SD ledRojo(); //activar led rojo delay(250); apagarLeds(); delay(250); } } //fin del if (SD.begin(53)== 0) //Si llega hasta aquí, se ha abierto OK la tarjeta SD // ES OBLIGATORIO TENER SD introducida aunque NO LA UTILICEMOS Serial.println ("Iniciacion de SD correcta"); //FIN LECTURA DE LA TARJETA*** } //Fin void setup() //-----------------------------------------------//-------------------COMIENZO LOOP --------------void loop() { //---------------------------------------------------------Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

81

//----dirección para hacer pruebas DE LA PS2X-----------//dirección: https://www.luisllamas.es/controla-arduino-con-el-mando-inalambrico-de-la-ps2/ //---MENSAJE Y DESPLAZAMIENTO A HOME-----Mensaje_Retorno_Posicion (); //---------------------------------------while (1) // Bucle infinito. { //----------instrucciones que permiten encender el led de iluminación---//------------- a través del pulsador del joystick 4---------------------//--------------------------------------------------------------------------------------------//-- VA FENOMENAL incluso si el led estuviese ON desde la PS2X--// puedo, desde la mesa, al primer toque, apagarlo.... //"pulsador_joystick4" en variable global conectado al pin 38 //no hay debounce, NO se necesita. ok esta rutina me apoyo en // https://www.prometec.net/condicionales-botones/ flag_Led = digitalRead(pulsador_joystick4); //conectado a pulsador de joystick if (flag_Led != estado_anterior) //hay cambio : Han pulsado o soltado { if (flag_Led == LOW) //Al pulsar botón cambiar led, pero no al soltar digitalWrite(led, !digitalRead(led)); estado_anterior = flag_Led ; // Para recordar el ultimo valor } //---------------------------fin activación led desde el pulsador joystick 4------------------//-------------------------------------------------------------------------------------------//----------------------------------------------------------------------------------//--------------CONTROL DESDE EL MANDO DE LA PS2X -------------//----------------------------------------------------------------------------------if ((digitalRead(Interruptor_PS2X_Joystick)== HIGH)and (digitalRead(Interruptor_VB17)== HIGH)) { //----activa (por NIVEL 0) el led indicador bajo el interruptor // digitalWrite (led_verde_bajo,LOW); //pin 39 es el led bajo el interruptor de VB17 No me hace falta... digitalWrite (activar_led_uso_ps2x,LOW); //pin 40 //--------------------------------------------------------//-----Debe haber al menos una lectura del módulo cada segundo.----ps2x.read_gamepad(); //----------------------------------------------------------------//JOYSTICK 1 PS2K //motores M1_D + M1_I SUBE-BAJA PINZA..("..PSS_LY valor 8") En Driver D1 - Joystick 1 Analog A0 JoystickPS2X(PSS_LY,0,2,22,23,'A'); JoystickPS2X(PSS_LX,1,3,24,25,'B'); //ROTAR pinza a DCHA motores M1_D y "M1_I". //-----cambio posición PSS_RX por PSS_RY para igualar a los de la mesa-----------------------//JOYSTICK 2 PS2K //motor M2. ("..PSS_RY valor 6") En Driver D2 - Joystick 3, Analog A3.

JoystickPS2X(PSS_RY,3,4,26,27,'D'); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

82

//motor M3. ("..PSS_RX valor 5") En Driver D2 - Joystick 3, Analog A4.

JoystickPS2X(PSS_RX,4,5,28,29,'E'); //-----------------AQUÍ UTILIZO PULSADORES por no haber más que 2 Joystick------------------// --Para el control del tercer motor con los pulsadores // PSB_PAD_RIGHT Y LEFT que mientras no se pulsen se lee valor 0 y al ser //pulsados su valor analogico oscila de 0 a 255. //--------------------ABRIR CERRAR PINZA CON PULSADORES ANALOGICOS de la PS2X----------------//PULSADORES "PSAB_PAD_UP" y "PSAB_PAD_DOWN" por no haber más que 2 Joysticks en la PS2K //Motor1_A_C de Abrir-Cerrar pinza.. En Driver D3 - pulsadores analogicos, Analog A2.

JoystickPS2X(PSAB_PAD_UP,2,6,30,31,'C'); JoystickPS2X(PSAB_PAD_DOWN,2,6,30,31,'C'); //------------------------------------------------------------------------//----------------------M4 GIRA COLUMNA CON PULSADORES ANALOGICOS de la PS2X----------------//PULSADORES "PSAB_PAD_RIGHT" y "PSAB_PAD_LEFT" por no haber más que 2 Joysticks en la PS2K //motor M4 Gira columna a dcha.("..PAD_RIGHT valor 10") JoystickPS2X(PSAB_PAD_RIGHT,5,44,32,33,'F'); //motor M4 Gira columna a Izqda. ("..PAD_LEFT valor 9") JoystickPS2X(PSAB_PAD_LEFT,5,44,32,33,'F'); //------------------------------------------------------------------------}// fin del if ((digitalRead(Interruptor_PS2X_Joystick)== HIGH)and .... //-----------------------------------------------------------------------------//-----------------------------------------------------------------------------------------------//-------------- CONTROL DESDE JOYSTICK MESA CONTROLADORA------------//-----------------------------------------------------------------------------------------------if ((digitalRead(Interruptor_PS2X_Joystick)== LOW) and (digitalRead(Interruptor_VB17)== HIGH)) { //----desactiva el indicador de uso por PS2X poniendolo a nivel 1 ya que este se activa NIVEL 0 //pin 39 es el led bajo el interruptor de VB17 No me hace falta pero hice agujero... digitalWrite (led_verde_bajo,LOW); //pin 40, DESACTIVA EL LED DE USO POR PS2X (este led se activa por nivel alto) digitalWrite (activar_led_uso_ps2x,HIGH); //--------------------------------------------------------//----------------ACTUALIZADO en joystick 1 sube-baja y rota pinza (A0 Y A1)-----------------//PONGO 15 como valorPSS por seguridad que no hay nada con esa numeración //en los comandos del mando de la playStation JoystickPS2X(15,0,2,22,23,'A'); //motor "M1_D" + M1_I .En Driver D1 - Joystick 1 Analog A0 //ROTAR pinza, motores M1_D y "M1_I".En Driver D1 - Joystick 2, Analog A1 JoystickPS2X(15,1,3,24,25,'B'); //motor M1_A_C.En Driver D3 - Joystick 2, Analog A2. Le asigno C al motor de Abrir-Cerrar pinza.

JoystickPS2X(15,2,6,30,31,'C'); //motor M2. En Driver D2 - Joystick 3, Analog A3. Le asigno D al motor del Brazo 1 el de sube/baja pinza JoystickPS2X(15,3,4,26,27,'D'); //motor M3. En Driver D2 - Joystick 3, Analog A4. le asigno E al motor del Brazo 2 sube/Baja.

JoystickPS2X(15,4,5,28,29,'E'); //motor M4. En Driver D3 - Joystick 4, Analog A5.le asigno F al motor que Gira la Columna a izqda-Dcha.

JoystickPS2X(15,5,44,32,33,'F'); //nota este último utilizo el pin 44 como señal PWM } Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

83

//-------------------------------------------------------------------------------------------------------//---------------------------------------------------------------------------------------------//+++ los valores para CONTROL DESDE EL PC con VISUAL BASIC 2017 +++ //---------------------------------------------------------------------------------------------//-------------------------------------------------------------------------------------------------------//--Si pin 13 el (19 no esta mal soldado) a nivel LOW, manejo desde PC con programa hecho en VB2017-if (digitalRead(Interruptor_VB17)== LOW)

// pin 13 INTERRUPTOR PARA CONTROL VB17

//----desactiva el indicador de uso por PS2X poniendolo a nivel 1 ya que este se activa NIVEL 0 --digitalWrite (led_verde_bajo,HIGH); //pin 39 es el led bajo el interruptor de VB17 No me hace falta pero hice agujero... digitalWrite (activar_led_uso_ps2x,HIGH);//pin 40 // DESACTIVA EL LED DE USO POR PS2X (este led se activa por nivel alto) //--------------------------------------------------------{ //Si 'J' ir a HOME. SI 'M'control Manual. si 'N' control Automático valorString = ""; //lo inicializo el string valorInt = 0; // // -------Si el puerto serie está disponible -------if ((Serial.available() > 0) or(FlagRepeticionCicloAuto == 'r')){ {

while(Serial.available() > 0) //para que no lea si no hay algo que leer c = Serial.read(); //lee mientras haya datos dato_puerto_serial = c; valorString.concat(c); // concantena los caracteres delay(10); //Tiempo lectura

//--tiempos para secuenciación seleccionados desde el combobox del programa VB17-if ((valorString.equals("10"))or (valorString.equals("12"))or (valorString.equals("14")) or(valorString.equals("16"))or (valorString.equals("18")) or (valorString.equals("20"))) { dato_puerto_serial = 'N'; //NOTA la 'N' no la utilizo en el programa //pero sí para darle un valor que no sea ninguno de los valores utilizados } // fin if ((valorString.equals("10")) or ...) } //fin while (Serial.available() > 0) //-------------------------------------------if(dato_puerto_serial =='P') { parada_emergencia(); //para poder anular la accion repetitiva del ciclo automático FlagRepeticionCicloAuto = 'p'; dato_puerto_serial = 'M'; } //------------------------------------------//---lectura Potenciómetros posición----Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

84

//CREO QUE el delayMicrosconds(100); (tiempo entre lecturas analógicas) //PODRÍA PONERLO AL FINAL DE ESTAS INSTRUCCIONES POR SER PINES DISTINTOS //pero lo dejo de momento por ir todo OK. //-------------------------------------------sensorValue1 = analogRead(sensorPin1); //M1_D Entrada Analogica A6 delayMicroseconds (100); //Tiempo para la conversión Analógica/Digital sensorValue2 = analogRead(sensorPin2); //M1_I Entrada Analogica A7 delayMicroseconds (100); sensorValue3 = analogRead(sensorPin3); //M1_A_C Entrada Analogica A8 delayMicroseconds (100); sensorValue4 = analogRead(sensorPin4); //M2 Entrada Analogica A9 delayMicroseconds (100); sensorValue5 = analogRead(sensorPin5); //M3 Entrada Analogica A10 delayMicroseconds (100); sensorValue6 = analogRead(sensorPin6); //M4 Entrada Analogica A11 delayMicroseconds (100); int a1=0;int a2=0; int a3=0; int a4=0;int a5=0; int a6=0; //PODRÍA HACERLOS MEJOR GLOBALES //-------------------------------------------------------------------//-------------A HOME desde el VISUAL BASIC-----------//------------------------------------------------------------------//instrucciones para hacer volver a casa a los motores //al recibir por puerto serie la letra 'J' if(dato_puerto_serial == 'J') { /* NUEVO: motores y pines de los joystick de control + potenciómetros de posición de dichos motores.

-------------- Entradas Analógicas de Control-----------MOTOR PINANALOGICO POTX ACCIÓN M1_D A0 A6 Sube/baja pinza (Activa M1_D+ M1_I) M1-I A1 A7 Rota pinza M1_A_C A2 A8 Abre-Cierra Pinza M2 A3 A9 Motor 2 sube-baja M3 A4 A10 Motor 3 sube Baja M4 A5 A11 Motor 4 de columna-base ---------------------------------------------------------------------------*/ //------Desplazar los ejes a "Home (.......) -----------//----------------------------------------------------------------------------------//tengo otra forma DE IR A HOME pero usando la librería myPID (....9) // no la utilizo por serme menos precisa q usando mi propia función "int Home(...)" // ver instrucciones al final del programa en la "RECOPILACIÓN INSTRUCCIONES DE AYUDA" // en el archivo en arduino ***.ino //-----------------------------------------------------------------------------------------//int Home (int PotMotorX,String motor,int pinPWM,int int1,int int2) //letra D Preferible en primer lugar el motor M2 DEL BRAZO DE LA PINZA // se evita que si el M2 esta muy bajo y el giro del 4 es grande //choque la pinza con la base del robot

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

85

Home (9,"motor M2",4,26,27); // letra D está en primer lugar como seguridad Home (6,"motor M1_D",2,22,23); //Motor M1_D Home (7,"motor M1_I",3,24,25); //Motor M1_I

letra A letra B

Home (8,"motor M1_A_C",6,30,31); //Motor M1_A_C letra C Home (10,"motor M3",5,28,29); //Motor M3

letra E

Home (11,"motor M4",44,32,33); //Motor M4

letra F

//------------------------------------------------------dato_puerto_serial ='M'; //para que no vuelva repetir "J" //------------------------------------------------------} // fin del if(dato_puerto_serial == 'J') de ir a HOME //---------------------------------------------------//+++PARA REPETIR CICLO AUTOMATICO DEL DATAGRID O NO. //SI es igual a 'r' repetir ciclo continuamente. hasta pulsar parar motores if (FlagRepeticionCicloAuto == 'r') //lo tengo en global { //modo auto control_modo=1; control=1; // total_progs = 0; //si lo dejo problemas si lo quito si hace repetición de ciclos autom Serial.print("CONNECTED"); }//fin del if (flag ==0) para repetir ciclo automatico sin parar. //----------------------------------------------------if(dato_puerto_serial == 'M'){ //modo manual control_modo=0; digitalWrite(ENA_1, HIGH); //del M1_D digitalWrite(ENA_2, HIGH); //del M1_I digitalWrite(ENA_3, HIGH); //del M2 digitalWrite(ENA_4, HIGH); //del M3 digitalWrite(ENA_5, HIGH); //del M1_A_C digitalWrite(ENA_6, HIGH); //del M4 Serial.print("CONNECTED"); FlagRepeticionCicloAuto = 'p'; //para parar el ciclo repetitivo al pulsar Manual FlagTiempoCiclo = 0 ;// inicio el flag a cero segundos } else if((dato_puerto_serial == '0')or (dato_puerto_serial == '2')or (dato_puerto_serial == '4') or (dato_puerto_serial == '6')or (dato_puerto_serial == '8') or (valorString == "10")or (valorString == "12")or (valorString == "14") or (valorString == "16") or (valorString == "18")or (valorString == "20")){ //modo auto Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

86

control_modo=1; control=1; Serial.print("CONNECTED"); FlagRepeticionCicloAuto = 'r'; //para repetir ciclo hasta pulsar: parar // los tiempos de espera entre ciclo y ciclo secuenciaciĂłn if (char(dato_puerto_serial) == '0') { String dato_puerto_serial = "0"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (char(dato_puerto_serial) == '2') { String dato_puerto_serial = "2"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (char(dato_puerto_serial) == '4') { String dato_puerto_serial = "4"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (char(dato_puerto_serial) == '6') { String dato_puerto_serial = "6"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (char(dato_puerto_serial) == '8') { String dato_puerto_serial = "8"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (valorString == "10") { String dato_puerto_serial = "10"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (valorString == "12") { String dato_puerto_serial = "12"; Proyecto: control del Robot Mentor V1, desarrollado en arduino por JoaquĂ­n Berrocal Piris. Enero 2019

87

FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (valorString == "14") { String dato_puerto_serial = "14"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (valorString == "16") { String dato_puerto_serial = "16"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (valorString == "18") { String dato_puerto_serial = "18"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } if (valorString == "20") { String dato_puerto_serial = "20"; FlagTiempoCiclo = dato_puerto_serial.toInt(); FlagTiempoCiclo = FlagTiempoCiclo*1000; Serial.println(FlagTiempoCiclo); } } //fin del else if (dato_puerto_serial == 'N' //--------------------------------------------------------------//++++SI EL MODO ES AUTOMÁTICO 'N' +++++++ //--------------------------------------------------------------if(control_modo==1){ //ejecución del bucle de automatización hasta //llegar a la última acción almacenada do{ //según el tipo de acción, se ejecuta una u otra función if(pos[control-1][0]==1){ //movimiento Setpoint1=pos[control-1][2]; Setpoint2=pos[control-1][3]; Setpoint3=pos[control-1][4]; Setpoint4=pos[control-1][5]; Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

88

Setpoint5=pos[control-1][6]; Setpoint6=pos[control-1][7]; if(pos[control-1][1]==3){ //movimiento indiferente posicion(0); }else if(pos[control-1][1]==4){ //movimiento coger posicion(1); }else if(pos[control-1][1]==5){ //movimiento vuelve de coger posicion(2); } }else if(pos[control-1][0]==2){ //mano if(pos[control-1][1]==1){ //cierra pinza(1); }else if(pos[control-1][1]==2){ //abre pinza(0); } } }while(control<=total_progs && control_modo==1); delay(FlagTiempoCiclo);// tiempo seleccionado desde el VB17 en segundos.

Serial.print(FlagTiempoCiclo); }//fin control_modo=1 == AUTOMATICO //---------------------------------------------------------------------//si la aplicación de control solicita programación,arduino //responde informando que está listo para leer la configuración //----------------------------------------------------// -------MODO MANUAL DESDE VB17----//---------------------------------------------------if(control_modo==0){ if(dato_puerto_serial=='Y'){ Serial.print("Y-ACCEPTED"); control_prog=1; //llama a función de configuración readString(); } //Si parametro=I (Información) se muestra //el valor actual de los sensores

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

89

if(dato_puerto_serial == 'I'){ cadena="M1_D:"+String(sensorValue1); cadena=cadena+";M1_I:"+String(sensorValue2); cadena=cadena+";M1_A_C:"+String(sensorValue3); cadena=cadena+";M2:"+String(sensorValue4); cadena=cadena+";M3:"+String(sensorValue5); cadena=cadena+";M4:"+String(sensorValue6); Serial.print(cadena); } //Si parametro=U (información) se envia la información //de los sensores para crear un punto de trayectoria if(dato_puerto_serial == 'U'){ cadena="PGM@M1_D:"+String(sensorValue1); cadena=cadena+";M1_I:"+String(sensorValue2); cadena=cadena+";M1_A_C:"+String(sensorValue3); cadena=cadena+";M2:"+String(sensorValue4); cadena=cadena+";M3:"+String(sensorValue5); cadena=cadena+";M4:"+String(sensorValue6); Serial.print(cadena); } //------------------------------------------------//Control MOTOR 1_A_C --> 'Q' PINZA ABRIR con TOPE en 400 //------------------------------------------------if (dato_puerto_serial == 'Q') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue3 = analogRead(sensorPin3); if(sensorValue3>M3MAX && dato_puerto_serial!='O'){ analogWrite(ENA_5,valorPWM_S); digitalWrite(IN1_5, LOW); digitalWrite(IN2_5, HIGH); }else{ //Girar al contrario para salir del Tope analogWrite(ENA_5,valorPWM_S); digitalWrite(IN1_5, HIGH); digitalWrite(IN2_5, LOW); delay (150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); }//fin if (dato_puerto_serial == 'Q') //------------------------------------------------//Control MOTOR 1_A_C --> 'W' PINZA CERRAR con TOPE en 634 //------------------------------------------------if (dato_puerto_serial == 'W') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue3 = analogRead(sensorPin3); if(sensorValue3<M3MIN && dato_puerto_serial!='O'){ analogWrite(ENA_5,valorPWM_S); digitalWrite(IN1_5, HIGH); digitalWrite(IN2_5, LOW); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

90

}else{ //Girar al contrario para salir del Tope analogWrite(ENA_5,valorPWM_S); digitalWrite(IN1_5, LOW); digitalWrite(IN2_5, HIGH); delay (150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); }//fin if (dato_puerto_serial == 'W') //------------------------------------------------//MOTORES M1_D + M1_I --> 'R' PINZA SUBIR con TOPE en 680 (120º) //------------------------------------------------if (dato_puerto_serial == 'R') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue1 = analogRead(sensorPin1); if((sensorValue1<M1MAX)&& dato_puerto_serial!='O'){ analogWrite(ENA_1,valorPWM_S); digitalWrite(IN1_1, HIGH); //corresponde a M1_D digitalWrite(IN2_1, LOW); analogWrite(ENA_2,valorPWM_S); digitalWrite(IN1_2, HIGH); //corresponde a M1_I digitalWrite(IN2_2, LOW); }else{ //Girar al contrario para salir del Tope analogWrite(ENA_1,valorPWM_S); digitalWrite(IN1_1, LOW); //corresponde a M1_D digitalWrite(IN2_1, HIGH); analogWrite(ENA_2,valorPWM_S); digitalWrite(IN1_2, LOW); //corresponde a M1_I digitalWrite(IN2_2, HIGH); delay (150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //------------------------------------------------//MOTORES M1_D + M1_I --> 'E' PINZA BAJAR con TOPE en 340 (60º) //------------------------------------------------if (dato_puerto_serial == 'E') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue2 = analogRead(sensorPin2); if((sensorValue2>M2MIN) && dato_puerto_serial!='O'){ analogWrite(ENA_1,valorPWM_S); digitalWrite(IN1_1, LOW); //corresponde a M1_D digitalWrite(IN2_1, HIGH); analogWrite(ENA_2,valorPWM_S); digitalWrite(IN1_2, LOW); //corresponde a M1_I digitalWrite(IN2_2, HIGH);

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

91

}else{ //Girar al contrario para salir del Tope analogWrite(ENA_1,valorPWM_S); digitalWrite(IN1_1, HIGH); //corresponde a M1_D digitalWrite(IN2_1, LOW); analogWrite(ENA_2,valorPWM_S); digitalWrite(IN1_2, HIGH); //corresponde a M1_I digitalWrite(IN2_2, LOW); delay (150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //--------------------------------------------------------------//ROTAR PINZA A DCHA ----> 'd' con TOPE de M1_I en 780 (137º) //--------------------------------------------------------------if (dato_puerto_serial == 'd') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue2 = analogRead(sensorPin2); if((sensorValue2<780)&& dato_puerto_serial!='O'){ analogWrite(ENA_1,valorPWM_S); digitalWrite(IN1_1,LOW); //corresponde a M1_D digitalWrite(IN2_1, HIGH); analogWrite(ENA_2,valorPWM_S); digitalWrite(IN1_2, HIGH); //corresponde a M1_I digitalWrite(IN2_2, LOW); }else{ //Girar al contrario para salir del Tope analogWrite(ENA_1,valorPWM_S); digitalWrite(IN1_1, HIGH); //corresponde a M1_D digitalWrite(IN2_1, LOW); analogWrite(ENA_2,valorPWM_S); digitalWrite(IN1_2, LOW); //corresponde a M1_I digitalWrite(IN2_2, HIGH); delay (150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //-------------------------------------------------------------//ROTAR PINZA A IZQDA ---> 'i' con TOPE de M1_D en 208 (37º) //-------------------------------------------------------------if (dato_puerto_serial == 'i') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue2 = analogRead(sensorPin2); if((sensorValue2>208)&& dato_puerto_serial!='O'){ Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

92

analogWrite(ENA_1,valorPWM_S); digitalWrite(IN1_1,HIGH); //corresponde a M1_D digitalWrite(IN2_1, LOW); analogWrite(ENA_2,valorPWM_S); digitalWrite(IN1_2, LOW); //corresponde a M1_I digitalWrite(IN2_2, HIGH); }else{ //Girar al contrario para salir del Tope analogWrite(ENA_1,valorPWM_S); digitalWrite(IN1_1, LOW); //corresponde a M1_D digitalWrite(IN2_1, HIGH); analogWrite(ENA_2,valorPWM_S); digitalWrite(IN1_2, HIGH); //corresponde a M1_I digitalWrite(IN2_2, LOW); delay (150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //+++++++++++++++FIN ROTAR PINZA ++++ //------------------------------------------------//MOTOR M2 --> 'A' BRAZO 1 SUBIR con TOPE en 33 (sobre 1023)(6º) //------------------------------------------------if (dato_puerto_serial == 'A') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue4 = analogRead(sensorPin4); if(sensorValue4>M4MAX && dato_puerto_serial!='O'){ analogWrite(ENA_3,valorPWM_S); digitalWrite(IN1_3, LOW); //SUBIR M2 digitalWrite(IN2_3, HIGH); }else{ //Girar al contrario para salir del Tope analogWrite(ENA_3,valorPWM_S); digitalWrite(IN1_3, HIGH); digitalWrite(IN2_3, LOW); delay (150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //------------------------------------------------//MOTOR M2 --> 'S' BRAZO 1 BAJAR con TOPE en 611 (sobre 1023)(108º) //------------------------------------------------if (dato_puerto_serial == 'S') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue4 = analogRead(sensorPin4); if(sensorValue4<M4MIN && dato_puerto_serial!='O'){ analogWrite(ENA_3,valorPWM_B); digitalWrite(IN1_3, HIGH); //BAJAR M2 digitalWrite(IN2_3, LOW); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

93

}else{ //Girar al contrario para salir del Tope analogWrite(ENA_3,valorPWM_S); digitalWrite(IN1_3, LOW); digitalWrite(IN2_3, HIGH); delay (150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //------------------------------------------------//MOTOR M3 --> 'D' BRAZO 2 SUBIR con TOPE en 796 (sobre 1024)(140º) //------------------------------------------------if (dato_puerto_serial == 'D') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue5 = analogRead(sensorPin5); if(sensorValue5<M5MAX && dato_puerto_serial!='O'){ analogWrite(ENA_4,(valorPWM_S)); //Partimos de 200 ó 180 y le incremento en + 25 digitalWrite(IN1_4, HIGH); //FINALMENTE NO INCREMENTO EN +25 POR PROBLEMAS EN VB17 digitalWrite(IN2_4, LOW); }else{ //Girar al contrario para salir del Tope analogWrite(ENA_4,valorPWM_S); digitalWrite(IN1_4, LOW); digitalWrite(IN2_4, HIGH); delay(150); // Tiempo necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //------------------------------------------------//MOTOR M3 --> 'F' BRAZO 2 BAJAR con TOPE en 242 (sobre 1024)(43º) //------------------------------------------------if (dato_puerto_serial == 'F') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue5 = analogRead(sensorPin5); if(sensorValue5>M5MIN && dato_puerto_serial!='O'){ analogWrite(ENA_4,valorPWM_B); digitalWrite(IN1_4, LOW); digitalWrite(IN2_4, HIGH); }else{ //Girar al contrario para salir del Tope analogWrite(ENA_4,valorPWM_S); digitalWrite(IN1_4, HIGH); digitalWrite(IN2_4, LOW); delay(150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //------------------------------------------------//MOTOR M4 --> 'Z' COLUMNA A DCHA el motor M4 A 255 //con TOPE en 180 (sobre 1023)(32º) //------------------------------------------------Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

94

if (dato_puerto_serial == 'Z') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue6 = analogRead(sensorPin6); if(sensorValue6<M6MAX && dato_puerto_serial!='O'){ analogWrite(ENA_6,255); digitalWrite(IN1_6, HIGH); digitalWrite(IN2_6, LOW); }else{ //Girar al contrario para salir del Tope analogWrite(ENA_6,valorPWM_S); digitalWrite(IN1_6, LOW); digitalWrite(IN2_6, HIGH); delay(150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //------------------------------------------------//MOTOR M4 --> 'X' COLUMNA A IZQDA con TOPE en 842 (sobre 1023)(148º) //------------------------------------------------if (dato_puerto_serial == 'X') { do{ dato_puerto_serial = Serial.read(); if(dato_puerto_serial=='O')break; sensorValue6 = analogRead(sensorPin6); if(sensorValue6>M6MIN && dato_puerto_serial!='O'){ analogWrite(ENA_6,255); digitalWrite(IN1_6, LOW); digitalWrite(IN2_6, HIGH); }else{ //Girar al contrario para salir del Tope analogWrite(ENA_6,valorPWM_S); digitalWrite(IN1_6, HIGH); digitalWrite(IN2_6, LOW); delay(150); // necesario para salir del Tope } }while(dato_puerto_serial!='O'); } //------------------------------------------------//LAMPARA LED PINZA --> ON 'L' y OFF 'K' //------------------------------------------------if (dato_puerto_serial == 'L') { digitalWrite(led, HIGH); }else if (dato_puerto_serial == 'K') { digitalWrite(led, LOW); //pin 43 conectado con el led de la mano } //Cuando no se recibe información útil, se recibirán 'O' //En caso de recibir 'O' --> se paran todos los motores if (dato_puerto_serial == 'O') { digitalWrite(IN1_1, LOW); digitalWrite(IN2_1, LOW); digitalWrite(IN1_2, LOW); Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

95

digitalWrite(IN2_2, LOW); digitalWrite(IN1_3, LOW); digitalWrite(IN2_3, LOW); digitalWrite(IN1_4, LOW); digitalWrite(IN2_4, LOW); digitalWrite(IN1_5, LOW); digitalWrite(IN2_5, LOW); digitalWrite(IN1_6, LOW); digitalWrite(IN2_6, LOW); } }//fin control_modo=0 }//fin if puerto serial disponible }// fin if (digitalRead(Interruptor_VB17)== LOW) para el uso con VB17 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ } //fin del While (1) para que esté en ciclo de lectura de los tres interruptores //uso mesa / uso PS2X / uso VB17 y actuar conforme cual este activado } //fin del void loop() // ++++++++++++++++++++FIN DEL PROGRAMA++++++++++++++++++++++++++ /* nota: en el programa en arduino tengo a partir del “FIN DEL PROGRAMA” una serie De instrucciones que me han ayudado en la realización y prueba del programa, tales como conocer posiciones y valores de los joystick, potenciómetros etc. */

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019

96