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.

Proyecto: control del Robot Mentor V1, desarrollado en arduino por Joaquín Berrocal Piris. Enero 2019 1
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);

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"

+ 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
*****************************************************
-. 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

-. Conexión de los Drivers amplificadores y de los Joystick a la arduino Mega:
Nº DRIVER L298N Joystick PWM JOYS PotX IN1 IN2 IN3 IN4 Letra
Nº 1
Motor M1_D (out 1-2) Nº1 (A0)
(A1)
2 A0 A6 22 23 A
Motor M1_I (out 3-4) 3 A1 A7 24 25 B
Nº 2
Motor M2 (out 1-2) Nº3 (A3) 4 A3 A9 26 27 D
Motor M3 (out 3-4) Nº3 (A4) 5 A4 A10 28 29 E
Nº 3
Motor M1_A_C ( 1-2) Nº2 (A2) 6 A2 A8 30 31 C
Motor M4 (out 3-4) Nº4 (A5) 44 A5 A11 32 33 F
M1_D + M1_I Rotar Nº1 (A1) 2-3 A1 A7 22 23 24 25 B
*Joystick* Función
Nº 1
A0 Activa a la vez M1_D y M1_I (motores de la pinza)
A1
Activa M1_D y M1_I (Rotar pinza)
Sólo utiliza eje Horizontal
Nº 2
Eje (X)
A2 Activa M1_A_C (abrir cerrar pinza)
Nº 3
A3 Activa M2 ( Sube/Baja brazo 1)
A4 Activa M3 ( Sube/Baja brazo 2)
Nº 4
Eje (X)
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

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

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

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

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

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

VERIFICACIÓN DE QUE GIRAN CORRECTAMENTE LOS MOTORES

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

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

ARREGLO Y PUESTA A PUNTO DE LA PINZA
PINZA ESTROPEADA PINZA ARREGLADA

CONTRUCCIÓN DE LA CAJA CONTROLADORA PARA LOS JOYSTICK

INTERIOR DE LA CAJA CONTROLADORA

MONTAJE FINAL DE LA CAJA CONTROLADORA

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..!!!)

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.

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

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

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.

YA MONTADA SOBRE LA ARDUINO MEGA 2560 R3

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

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

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

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

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

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.

C.Redondo
Borne
Color
Arduinopin
CONECTOR DE 26 PINES MACHO
DE LA CAJA DE CONTROL Y DEL ROBOT MENTOR
FUNCIÓN
Verificado
1 V A6 Pot 1_D salida analógica a A6 √
2 G A7 Pot 1_I salida analógica a A7 √
3 Na A8 Pot1_A_C abre cierra pinza a A8 √
4 Ma A9 Pot 2 salida analógica a A9 √
5 Vi A10 Pot 3 salida analógica a A10 √
6 N A11 Pot 4 salida analógica a A11 ESTA SUELTO DEL C1 soldarlo √
7 M (-) M1_D de la pinza Dcha a Out2 Driver 1 √
8 R (+)M1_D de la pinza Dcha a Out1 Driver 1 √
9 B (-) M1_I de la pinza Izqda a Out4 Driver 1 √
10 Vi (+) M1_I de la pinza Izqda a Out3 Driver 1 √
11 Na (-) M2 del Brazo-de Pinza 1 a Out2 Driver 2 √
12 G (+) M2 del Brazo-de Pinza 1 a Out1 Driver 2 √
13 V (-) M3 del Brazo 2 a Out4 Driver 2 √
14 G (+) M3 del Brazo 2 a Out3 Driver 2 √
15 V (-) M1_A_C de la pinza a Out2 Driver 3 √
16 Am (+) M1_A_C de la pinza a Out1 Driver 3 √
17 Az (-) M4 del Brazo-Columna 3 a Out4 Driver 3 √
18 Am
(+) M4 del Brazo-Columna 3 a Out3 Driver 3 23 filas
conexiones
√
19 B/R 43
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.
√
20 R 9 LED ROJO RGB √
21 V 10 LED VERDE RGB √
22 Az 11 LED AZUL RGB √
23
R-
Az
+5v
+5v Procedente del L298 N1 a ficha empalme interna, para alimentar
potenciómetros
√
24
R-
Az
+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.
√
25
M-
N
GND
GND de los 14,7V Transformador interno. (Tengo Todas las Masas Unidas)
√
26 NO CONEXIONADO

Manguera
Cable
Borne
Color
Verificado
Cableado
1
1 N √
2 Ma √
3 R √
4 Na √
5 Am √
6 V √
7 Az √
8 Vi √
9 G √
10 B √
Cableado
2
11 N √
12 Ma √
13 R √
14 Na √
15 Am √
16 V √
17 Az √
18 Vi √
19 G √
20 B √
Cableado
3
21 N √
22 Ma √
23 R √
24 (+12v) Na √
25 (-) Am √
26 V √

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

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

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

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

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

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

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 Input2 Acción
0 0 Parada del motor
1 0 El motor gira hacia adelante
0 1 El motor gira hacia atrás
1 1 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.

√ 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

ESQUEMA L298N :
*/

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

//--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
//-----------------------------------------
// ----------------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
// motor 3 M3---> Sube Baja brazo de la columna; Brazo 2
const int IN1_4 = 28; //pin 28-59 nivol 1 o 0 para cambiar sentido giro
//---------------Driver D3 manejado desde el Joystick 1 y 4-----------
// motor M1_A_C ---> Base
// motor M4 ---> Base

//---------------------------------------------------------------------
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 mem_sensor2=0;
//----M1_A_C-----
int mem_sensor3=0;
//-------M2------
int mem_sensor4=0;
//-------M3------
int mem_sensor5=0;
//-------M4------
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; //M1_D
double Setpoint2, Input2, Output2,Setpoint_old2; //M1_I
double Setpoint3, Input3, Output3,Setpoint_old3; //M1_A_C
double Setpoint4, Input4, Output4,Setpoint_old4; //M2
//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

double consKp1=0.2, consKi1=0.05, consKd1=0; // 0.2 0.05
PID myPID1(&Input1, &Output1, &Setpoint1, consKp1, consKi1, consKd1,DIRECT); //M1_D
PID myPID2(&Input2, &Output2, &Setpoint2, consKp2, consKi2, consKd2,DIRECT); //M1_I
PID myPID3(&Input3, &Output3, &Setpoint3, consKp3, consKi3, consKd3,DIRECT); //M1_A_C
PID myPID4(&Input4, &Output4, &Setpoint4, consKp4, consKi4, consKd4,DIRECT); //M2
//----------------------------------------------------------------
//--------------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

* - 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(verde,LOW);
digitalWrite(azul,HIGH);
}
void apagarLeds() //activa led Azul
{
digitalWrite(verde,LOW);
digitalWrite(azul,LOW);
}
//-------------------------------------------------------------------
// ++++++FUNCIÓ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-----------
//-------------------------------------------------------------------

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

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);
}
// ----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);
//--------calculo de A0 y A1 , (subir/bajar) y Rotar pinza,---------

//(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
//lectura de A1 De Rotar pinza
valorA1 = analogRead(A1); //le el valor del joystick 1 entrada Analogica A1
}//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 --> Secuenciar-
analogWrite(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);

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

{
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 (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 (2,LOW);//(pinPWM,LOW);
//---- PONGO TAMBIÉN EL ANULAR LAS SALIDAS DEL M1_I que por defecto no se pondrían --
digitalWrite (24,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 (2,LOW);//(pinPWM,LOW);
//---- PONGO TAMBIÉN EL ANULAR LAS SALIDAS DEL M1_I que por defecto no se pondrían --
}
//-------------------------------------------------------------------------
//---------------------------------------------------------
//--------------------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 (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);
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
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)))

{
analogWrite (2,valorPWM_S); //este activa pinPWM 2
digitalWrite (22,HIGH);
analogWrite (3,valorPWM_S); //y este el pinPWM 3
}
//---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 (3,valorPWM_S); //y este el (pinPWM +1)pinPWM 3
}
//---------------------------------------------------------
//----------------------------------------------------------
//---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)))
{
}
//---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)))
{
}
//----------------------------------------------------------

// ----------------------------------------------------------------
// -------- TOPES -----
//-----------------------------------------------------------------
servoVal = analogRead (6); //
servoVal = map(servoVal,0,1023,0,180);
Serial.print("MOTOR 1_D ");
Serial.println(servoVal);
servoVal = analogRead (7);
servoVal = map(servoVal,0,1023,0,180);
Serial.print("MOTOR 1_I ");
Serial.println(servoVal);
// ---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---
//-------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
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---
//-------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
delay(150);// tiempo para q retroceda
}
//----------Fin de EVITAR TOPES DE SUBIDA y BAJADA de la pinza M1_D + M1_I----------

//------------------------------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----
//delay(25);
//-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope--------
analogWrite (pinPWM,valorPWM_S);
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----
//-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope--------
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---

//-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope-----------------
delay(150);
}
//A4 corresponde al motor M3 brazo columna
if ((valorPotX > 140)and (pinAnalogico == 4))
{
//-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope----------------
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))
{
// delay(25); //espera a q se pare
//-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope--------------
delay(150);
}
if ((valorPotX > 148) and (pinAnalogico == 5))
{
//-------LO ACTIVA EN SENTIDO CONTRARIO para salir del Tope---------------

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
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)
{
//------------------------
Serial.print (motor);
Serial.print (" ");
//------------------------
//-------------------Posición HOME 90º-----------------
if (servoVal== 90)
{

Serial.print("ESTA EN POSICION ");
Serial.println (servoVal);
delay(150);
//de nuevo analizar y actuar en consonancia:
}
//--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)
{
}
// 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"))
{

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

---------------------------------------------------------
*/
//------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 letra A
Home (7,"motor M1_I",3,24,25); //Motor M1_I 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);

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

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

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

{
//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
//-------------------------------------------------------------
if (servoVal != valorAnterior) //para evitar repetir ciclos
{
//------------------------
Serial.print ("motor: ");
Serial.print (PotMotorX); //motor 0 a 4
Serial.print ("--");
//------------------------
//--Con la función "abs" obtengo valor global obviando el signo +/-
// Para dar una histéresis de +-1º
if (abs(servoVal - valorInt) < 2)
{
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
}

if (abs(servoVal - valorInt) < 2)
{
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))
{
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))
{
digitalWrite (int1,HIGH); //LOW
digitalWrite (int2,LOW); //HIGH
{
}
}
//--------------- SI ES MAYOR LA SEPARACION A +3-----Esta corresponden a valorPWM_B---
if (servoVal >= (valorInt+3))
{
digitalWrite (int1,LOW);//HIGH
digitalWrite (int2,HIGH);//LOW
{
}
}
//PARA REDUCIR VELOCIDAD AL M3 AL BAJAR
if (servoVal <= (valorInt+3)and (servoVal >= (valorInt)and(PotMotorX == 10)))
{

}
if (servoVal <= (valorInt+3)and (servoVal >= (valorInt)))
{
{
}
}
} // 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;

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'){
}
}else if(countbucle==2){
if(str2[0]=='C'){
}else if(str2[0]=='A'){
}else if(str2[0]=='I'){
}else if(str2[0]=='L'){
}else if(str2[0]=='T'){
}
}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

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_5, HIGH); //pin PWM de M1_A_C
digitalWrite(IN1_1, 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);
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;

if(control_trayectoria==1){
control_prioridad=1;
}else if(control_trayectoria==2){
}else{
}
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-----
c1=0;
}else{
Setpoint_old1=analogRead(sensorPin1);
c1=1;
}
//--------------------------------------
//----M1_I-----
c2=0;
}else{
c2=1;
}
//-------------------------------------
//----M1_A_C-----
c3=0;
}else{

c3=1;
}
//----M2-----
c4=0;
}else{
Setpoint_old4 = analogRead(sensorPin4);
c4=1;
} //he añadido c1==1 para poder incluir el posicionamiento del MOTOR 1
if(c1==1 && c2==1 && c3==1 && c4==1){
}
}//fin control prioridad==0
if(control_prioridad==1 || control_prioridad==2){
//----M3-----
c5=0;
} else{
c5=1;
}
if(c5==1){
}
// //fin control prioridad==1 o 2
//----M4-----
c6=0;
} else{
c6=1;
}
if(c6==1){
}

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

myPID){
double gap = abs(Setpoint-Input2); //distancia entre el punto actualy el definitivo
}else{
}
}else{
}
myPID.Compute();
if(var!=var_old){
var2=Output2;
var_old=var;
}
}
//------------------------------------------------
//------------FUNCION GIRA3--------------
//------------------------------------------------
//-----------------------------------------------
//MOTOR M1_A_C control desde VB17 (Abierta 400 cerrada 634)
//-----------------------------------------------
//Activa según la posición deseada
myPID)
{
}else{
}
}else{
}
myPID.Compute();

if(var!=var_old){
var2=Output3;
var_old=var;
}
}
//------------------------------------------------
//------------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
myPID)
{
}else{
}
}else{
}
myPID.Compute();
if(var!=var_old){
var2=Output4;
var_old=var;
}
}
//------------------------------------------------------
//----------------FUNCION GIRA5----------------
//------------------------------------------------------
//MOTOR M3 control desde VB17 (arriba 796 abajo 242)
//-----------------------------------------------
myPID)
{

}else{
}
}else{
}
myPID.Compute();
if(var!=var_old){
var2=Output5;
var_old=var;
}
}
//--------------------------------------------------
//------------FUNCION GIRA6----------------
//--------------------------------------------------
//MOTOR M4 control desde VB17 ( a dcha 842 a izqda 180)
//-----------------------------------------------
myPID)
{
}else{
}
}else{
}
myPID.Compute();
if(var!=var_old){
var2=Output6;
var_old=var;
}

}
//*****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
// Control L298N del Motor M1_I
//---------DRIVER 2 L298 ----------------------------
// Control L298N del Motor M2
//---------DRIVER 3 L298 ----------------------------
// Control L298N del Motor M1_A_C

Proyecto robot mentor v1 enero_19_por_joaquin berrocal piris

Proyecto robot mentor v1 enero_19_por_joaquin berrocal piris

Recomendados

Recomendados

Más contenido relacionado

La actualidad más candente

La actualidad más candente (20)

Similar a Proyecto robot mentor v1 enero_19_por_joaquin berrocal piris

Similar a Proyecto robot mentor v1 enero_19_por_joaquin berrocal piris (20)

Más de joaquinin1

Más de joaquinin1 (20)

Último

Último (13)

Proyecto robot mentor v1 enero_19_por_joaquin berrocal piris