Proyecto brazo robotico ksr10 enero marzo 2015 a 9 v-version_2

1
PROYECTO:
AUTOMATIZACIÓN DEL
BRAZO ROBÓTICO KSR10
Nombre del proyecto:
"Robotic_Arm_KSR10_y_SD.ino" (Versión 2)
(Realizado en Arduino 1.05.r2)
Autor: Joaquín Berrocal Piris
Fecha: final Marzo 2015
Autor : Joaquín Berrocal Piris

2
Al principio tengo una breve descripción y las fotos del proceso. Y al final, desde la página 29 a la 50, tengo
escrito todo el programa realizado en Arduino 1.05.r2
“”Está ajustado para una tensión de alimentación a los drivers L298N de 9 V DC.””
Direcciones descarga documento”Version 1”: http://issuu.com/joaquinin/docs/proyecto_brazo_robotico_ksr10__ener
http://www.slideshare.net/joaquinin1/proyecto-brazo-robotico-ksr10-enero-marzo-2015-46138135
/*proyecto "Robotic_Arm_KSR10_y_SD" (versión 2) .El código está más compacto y los motores con
Autor: JOAQUÍN BERROCAL PIRIS mayor Torque.
Fecha: finales de Marzo 2015
Videos en youtube: https://youtu.be/FPJ2WEjW-uY y en https://youtu.be/98JHhiSq_Go
/*proyecto "Robotic_Arm_KSR10_y_SD"
"""Versión 2""">>> más rápida mayor velocidad
Autor: JOAQUÍN BERROCAL PIRIS
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 la marca KSR10 de Velleman
con 5 motorcitos de 6 voltios, acepta incluso hasta 12V.
+Dispone de un interruptor para permitir 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 secuenciar los
movimientos grabados de forma continua mientras se mantenga activado.
será indicado mediante led Azul.
+ Cuando se activa el programa, lo primero que hará
será ir a posición HOME. que corresponde a posición
de 89º a 91º en los potenciómetros de los respectivos motores.
+ También cada vez q pulse cualquiera de los dos interruptores lo primero
será llevar el brazo a posición HOME.
por consiguiente hasta que no esté en esa posición no se permitirá
grabar ni secuenciar los movimientos. 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 1-2 para comunicar a través
de puerto serial mensajes varios entre ellos datos de posición.
la 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 +- 1º.
**No uso ninguna librería específica para la calibración de la posición de los motores+
**aunque hay una librería llamada "PID_v1.h" para el control de movimientos pero
no hago uso por no tener datos suficientes para su manejo.**
MATERIALES Y CONEXIONES:

3
-. Brazo robótico 50€ marca KSR10 de velleman
-. Le he colocado 5 potenciómetros 10K para control de posiciones
-. 5 Motores de 6 voltios DC
-. 3 Joystick para control movimiento de los 5 motores
-. 5 Potenciómetros control posición
****CONEXIÓN ANALOGICA DE LOS Joystick y Potenc******
Joystick ---- Potenciometro
motor 1 ---- A0 --------- A5
motor 2 ---- A1 --------- A6
motor 3 ---- A2 --------- A7
motor 4 ---- A3 --------- A8
motor 5 ---- A4 --------- A9
*****************************************************
-. 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 5 motores, en conjunto, no llegan a a sobrepasar 1 A. como mucho 1,5 A.
-. tarjeta SD para grabación de las secuencia 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.
conexión pines Tarjeta SD;
Ardu Mega Ard UNO
CS --------- 53 ------ 10 "en la ethernet shiel el 4"
clk/sck ---- 52 ------ 13
MOSI ------- 51 ------ 11
MISO ------- 50 ------ 12
-. Conexión de los amplificadores a la arduino Mega:
Los Pines PWM de arduino a la conexion ENA o ENB del L298N
los otros dos numero son las entradas in1 in2 del amplificador
conectadas a los pines de arduino nº 22 y 23 ...
pin PWM IN1 / IN2
Motor 1-----2 --- 22 / 23
Motor 2-----3 --- 24 / 25
Motor 3-----4 --- 26 / 27
Motor 4-----5 --- 28 / 29
Motor 5-----6 --- 30 / 31
-.(*los leds 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);
-. interruptor para grabar movimientos: al pin 7
-. interruptor para secuenciar movimientos grabados al pin 8
(la resistencia de los interruptores puestas a + Pull-up
al activar el pulsador conecto el pin 7 o el 8 a masa (-))
-.Niveles de giro máximo :
Tan solo tengo limites max permitidos por software al motor 1 que es la pinza
y al motor 5 encargado del giro transversal del brazo. el M 2-3-4 No les tengo límites.
Motor 1 de 108 patillas cerradas a 57º plenamente abiertas

4
Motor 2 de 50º a 126º
Motor 5 de 120º a 70º (Realmente acepta 127º a 63º pero para evitar
las inercias y que choque con palanca potenciómetro 5)
**** OTROS DATOS Interesante en la configuración del programa: ******
-. Posición HOME: valores potenciométros entre 89º a 91º . Es decir un +/- 1º respecto a 90º
para permitir una cierta histéresis que impida estén fluctuando el accionamiento de los
motores para mantenr la posición.
+Cuando se va acercando a 90º reduzco la velocidad entorno a 50 / 110 para mejor ajuste.
-. Posición JOYSTICK posición neutra de los joystick para evitar accionamientos indeseados
neutra de 80 a 94. todo en LOW / LOW / LOW (el enable y int 1 int 2 del amplificador L298N)
- Si el valor leído del joystic < 80 giro en un sentido: LOW / HIGH
- si el valor leído del joystic > 94 giro en sentido contrario: HIGH / LOW
En la función; Joystick ( int pinanalogico, int pinPWM, int int1, int int2, char letra)
tengo el accionamiento manual de los motores mas chequeo interruptores de grabar o secuenciar
desde esta función evito sobrepasar los grados de giro para el motor 1 de la pinza y del motor
5 del giro transversal del brazo.
-.en HOME valor global de accionamiento 180. Motor 1 a 80 Motor 2 a 150.
Y cuando se acerca a posición home 90º;
if (servoVal >= 83) and (servoVal < 89) and (motor == "motor 1")
y en if ((servoVal >= 92)and(servoVal <= 95) and (motor == "motor 1"))
velocidad mas lenta: Motor 1 a 50 y para Motor 2 a 100, para el resto de motores 110
-.en Joystic valor global de accionamiento 180, motor 1 a 80, motor 2 a 150
-.en ActivarMotores valor global de accionamiento 200, motor 1 a 90, motor 2 a 170
motor 5 a 200 lo puedo suprimir***
Para trabajar con LA LCD 4X16 utilizando 2 hilos el de reloj SCL y el de Datos
utilizar SDA --- 20 SCL -- 21 ("En este proyecto no la utilizo")
*/
F O T O S PROCESO CONSTRUCCIÓN

5
Modelo KSR10 DE vELLEMAN
MONTAJE COMPONENTES, Complicadillo

15
¿CÓMO COLOCAR LOS POTENCIÓMETROS DE POSCIÓN MOTORES? 1er intento

16
¡¡¡¡Segundo intento de colocación potenciómetros, algo más elaborada y más exitosa!!!

19
Se continúa haciendo pruebas….

20
Tablero de control con un sólo Joystick y dos potenciómetros, dos drivers L298N ..

22
Utilizo la placa ETHERNET por tener lector de micro tarjetas SD donde mi intención será grabar los movimientos
que desee utilizando los Joystick ara luego secuenciarlos de forma repetitiva y automática.
(Más adelante cuando me llegue la placa micro SD dejaré de utilizar la ETHERNET.)
COLOCACIÓN DE LOS NUEVOS POTENCIÓMETROS CON LOS QUE FINALMENTE SE QUEDARÁ EL PROYECTO.
(tardaron mucho de china, lo que supuso un problema para poder avanzar.)

25
ESTO YA ES OTRA COSA. MÁS ESTABLES Y FIELES LOS VALORES LEÍDOS

27
POR ÚLTIMO ME DECIDO A CONTROLAR TAMBIÉN EL MOVIMIENTO GIRATORIO CON EL MOTOR 5
(Gran complicación debido a no tener muy claro dónde y cómo fijarlo con su palanca de accionamiento)

29
/*proyecto "Robotic_Arm_KSR10_y_SD" (versión 2)
Fecha: finalización finales de Marzo 2015
Videos en youtube: https://youtu.be/FPJ2WEjW-uY y en https://youtu.be/98JHhiSq_Go
/*proyecto "Robotic_Arm_KSR10_y_SD"
"""Versión 2""">>> más rápida mayor velocidad
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 la marca KSR10 de Velleman
con 5 motorcitos de 6 voltios, acepta incluso hasta 12V.
+Dispone de un interruptor para permitir 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 secuenciar los
movimientos grabados de forma continua mientras se mantenga activado.
será indicado mediante led Azul.
+ Cuando se activa el programa, lo primero que hará
será ir a posición HOME. que corresponde a posición
de 89º a 91º en los potenciómetros de los respectivos motores.
+ También cada vez q pulse cualquiera de los dos interruptores lo primero
será llevar el brazo a posición HOME.

30
por consiguiente hasta que no esté en esa posición no se permitirá
grabar ni secuenciar los movimientos. 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 1-2 para comunicar a través
de puerto serial mensajes varios entre ellos datos de posición.
la 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 +- 1º.
**No uso ninguna librería específica para la calibración de la posición de los motores+
**aunque hay una librería llamada "PID_v1.h" para el control de movimientos pero
no hago uso por no tener datos suficientes para su manejo.**
MATERIALES Y CONEXIONES:
-. Brazo robótico 50€ marca KSR10 de velleman
-. Le he colocado 5 potenciómetros 10K para control de posiciones
-. 5 Motores de 6 voltios DC
-. 3 Joystick para control movimiento de los 5 motores
-. 5 Potenciómetros control posición
****CONEXIÓN ANALOGICA DE LOS Joystick y Potenc******
Joystick ---- Potenciometro
motor 1 ---- A0 --------- A5
motor 2 ---- A1 --------- A6
motor 3 ---- A2 --------- A7
motor 4 ---- A3 --------- A8
motor 5 ---- A4 --------- A9
*****************************************************
-. 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 5 motores, en conjunto, no llegan a a sobrepasar 1 A. como mucho 1,5 A.
-. tarjeta SD para grabación de las secuencia 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.
conexión pines Tarjeta SD;
Ardu Mega Ard UNO
CS --------- 53 ------ 10 "en la ethernet shiel el 4"
clk/sck ---- 52 ------ 13
MOSI ------- 51 ------ 11
MISO ------- 50 ------ 12
-. Conexión de los amplificadores a la arduino Mega:
Los Pines PWM de arduino a la conexion ENA o ENB del L298N
los otros dos numero son las entradas in1 in2 del amplificador
conectadas a los pines de arduino nº 22 y 23 ...
pin PWM IN1 / IN2
Motor 1-----2 --- 22 / 23
Motor 2-----3 --- 24 / 25
Motor 3-----4 --- 26 / 27
Motor 4-----5 --- 28 / 29

31
Motor 5-----6 --- 30 / 31
-.(*los leds 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);
-. interruptor para grabar movimientos: al pin 7
-. interruptor para secuenciar movimientos grabados al pin 8
(la resistencia de los interruptores puestas a + Pull-up
al activar el pulsador conecto el pin 7 o el 8 a masa (-))
-.Niveles de giro máximo :
Tan solo tengo limites max permitidos por software al motor 1 que es la pinza
y al motor 5 encargado del giro transversal del brazo. el M 2-3-4 No les tengo límites.
Motor 1 de 108 patillas cerradas a 57º plenamente abiertas
Motor 5 de 120º a 70º (Realmente acepta 127º a 63º pero para evitar
las inercias y que choque con palanca potenciómetro 5)
**** OTROS DATOS Interesante en la configuración del programa: ******
-. Posición HOME: valores potenciométros entre 89º a 91º . Es decir un +/- 1º respecto a 90º
para permitir una cierta histéresis que impida estén fluctuando el accionamiento de los
motores para mantenr la posición.
+Cuando se va acercando a 90º reduzco la velocidad entorno a 50 / 110 para mejor ajuste.
-. Posición JOYSTICK posición neutra de los joystick para evitar accionamientos indeseados
neutra de 80 a 94. todo en LOW / LOW / LOW (el enable y int 1 int 2 del amplificador L298N)
- Si el valor leído del joystic < 80 giro en un sentido: LOW / HIGH
- si el valor leído del joystic > 94 giro en sentido contrario: HIGH / LOW
En la función; Joystick ( int pinanalogico, int pinPWM, int int1, int int2, char letra)
tengo el accionamiento manual de los motores mas chequeo interruptores de grabar o secuenciar
desde esta función evito sobrepasar los grados de giro para el motor 1 de la pinza y del motor
5 del giro transversal del brazo.
-.en HOME valor global de accionamiento 180. Motor 1 a 80 Motor 2 a 150.
Y cuando se acerca a posición home 90º;
if (servoVal >= 83) and (servoVal < 89) and (motor == "motor 1")
y en if ((servoVal >= 92)and(servoVal <= 95) and (motor == "motor 1"))
velocidad mas lenta: Motor 1 a 50 y para Motor 2 a 100, para el resto de motores 110
-.en Joystic valor global de accionamiento 180, motor 1 a 80, motor 2 a 150
-.en ActivarMotores valor global de accionamiento 200, motor 1 a 90, motor 2 a 170
motor 5 a 200 lo puedo suprimir***
Para trabajar con LA LCD 4X16 utilizando 2 hilos el de reloj SCL y el de Datos
utilizar SDA --- 20 SCL -- 21 ("En este proyecto no la utilizo")
*/

32
#include <SD.h>
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; // variables a leer por los puertos analógicos
int valorAnterior;
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
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
//------PINES CONTROL L298N puente en H 2 Amp por canal
//motor 1 motor de pinzas
const int ENA_1 = 2; //pin 2 pulsos PWM control de velocidad
const int IN1_1 = 22;//pin 22-23 nivel 1 o 0 para cambiar sentido giro
const int IN2_1 = 23;
// motor 2
const int ENA_2 = 3;//pin 3 pulsos PWM control de velocidad
// motor 3
// motor 4
const int IN1_4 = 28;//pin 28-29 nivol 1 o 0 para cambiar sentido giro
// motor 5
const int ENA_5 = 6; //pin 6 pulsos PWM control de velocidad
byte FlagGrabar = 0;
byte FlagSecuenciar = 0;
int i = 0; //variable global
int valorInt ; //para convertir los valores de los grados a enteros y activar el motor
char c ;
String valorString = "";
//+++++CONJUNTO DE RUTINAS EMPLEADAS+++++++
//+++++++++++++++++++++++++++++++++++++++++
void ledRojo() //activa led rojo

33
{
digitalWrite(rojo,HIGH); //se enciende avisi 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
{
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);
}
// ----FUNCION JOYSTICK para manejo manual motores ----------
int Joystick (int pinAnalogico,int pinPWM,int int1,int int2,char letra)
{
int servoVal = analogRead (pinAnalogico);
servoVal = map (servoVal,0,1023,0,180);
delayMicroseconds(100); // tiempo para la conversion Analogica/digital son 100usg que son 0,1 ms.
//MODIFICAR LOS MARGENES SI FUERA NECESARIO
// ------- AQUÍ IRÍAN LAS INSTRUCCIONES donde comprobé -----------
//el fallo en la lectura de los interruptores por mala colocación
//de los parentesis
// ""la tengo guardada al final del programa con el nº 1"""
//----------------------------------------------------------------
// ++++++CHEQUEAMOS LOS INTERRUPTORES +++++
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 Grabacion --------
if ((digitalRead(Interruptor_grabar) == LOW)and (FlagGrabar) == 0) //pin 7 a cero para grabar movimientos

34
{
StopMotores(); // para los motores todos llevan 200 ms
//poner todos los motores parados hacer rutina
Serial.println("pin 7 a cero Grabar_movimientos");
//mensaje y retorno a la posicion inical de 90º tarda 5 a 6 sgdos.
Mensaje_Retorno_Posicion() ; // o mejor poner aquí mensaje de grabacion +++++++**********
//----lo primero borrar el anterior archivo de secuencias de movimiento---
SD.remove("posicmot.txt");
miarchivo = SD.open("posicmot.txt", FILE_WRITE);//nombre del archivo con los pasos grabados
FlagGrabar = 1; //flag de que se está grabando y no repetir funcion
} // Fin del if (Interruptor_grabar == 0)
if (digitalRead(Interruptor_grabar) == LOW)
{
int valorPotX = analogRead (pinAnalogico+5);//al Joystic A0 le corresponde el Potenciometro A5
valorPotX = map (valorPotX,0,1023,0,180);
Grabar(valorPotX,letra); // ir a grabar y luego mover los motores con joystick
} // fin del if (digitalRead((Interruptor_grabar) == LOW)
if ((digitalRead (Interruptor_grabar)== HIGH)and (FlagGrabar) == 1)//para que no cierre más el archivo
{
StopMotores(); //hay en él un delay de 200ms por lo que no es necesario el debounce
miarchivo.close(); //Cuando levante esperar 100 ms para el dobounce y cerrar archivo
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(); //hay en él un delay de 200ms por lo que no es necesario el deobunce
Serial.println("pin 8 a cero Secuenciar_movimientos");
Mensaje_Retorno_Posicion() ; // o mejor poner aquí mensaje de grabacion +++++++**********
FlagSecuenciar = 1; //flag de que se está secuenciando y no repetir funcion
} // 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))
if ((digitalRead (Interruptor_secuenciar)== HIGH)and (FlagSecuenciar) == 1)//para que no cierre más el archivo
{
StopMotores(); //hay en él un delay de 200ms por lo que no es necesario el deobunce
miarchivo.close();

35
FlagSecuenciar = 0; // Iniciamos el flag
apagarLeds();
}
//----------------------------------------------------
//------ INSTRUCCIONES PARA MOVER LOS MOTORES CON LOS JOYSTICK ---
Serial.print ("Joystick sensor ");
Serial.print (pinAnalogico);
Serial.print (" ");
Serial.print (servoVal);
Serial.println (" Grados");
// --CON EL FIN DE MEJOR CONTROL DE LA PINZA motor 1---
//--- reducir velocidad al cerrar y aumentar al abrir ---
int valorPWM = 180; //valor global para los motores
if ((pinAnalogico) == 0) // si es el motor 1 velocidad 80
valorPWM = 80 ;
if ((pinAnalogico) == 1) // si es el motor 2 velocidad 150
valorPWM = 150 ;
//para probar y garantizar manejo cómodo y el no bloqueo de la pinza
int valorPotX = analogRead (pinAnalogico+5);//al Joystick A0 le corresponde el Potenciometro A5
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.
if (servoVal>= 80 and (servoVal <= 94))
{
digitalWrite (int1,LOW);
digitalWrite (pinPWM,LOW);
Serial.print("ESTA EN POSICION ");
Serial.println (servoVal);
}
//---------------------------------------------
if (servoVal < 80)
{
analogWrite (pinPWM,valorPWM);
digitalWrite (int2,HIGH);
}
// -----Para evitar bloqueo por llegar a extremos Para M1 y M5 -------
// evita bloqueo excesivamente pinza cerrada, tiende a ABRIR pinza

36
if ((valorPotX > 108) and (pinAnalogico == 0))
{
//tiende a abrir para evitar bloqueo M1
analogWrite (pinPWM,90);
}
// evita que el M5 choque con patilla Potenciometro 5 margen 70º izda a 121º dcha
if ((valorPotX > 120) and (pinAnalogico == 4))
{
StopMotores();// para estabilizar y evitar que dañe potenciometro
}
//--------------------------------------------------------------------
if (servoVal > 94)
{
}
// -----Para evitar bloqueo por llegar a extremos Para M1 y M5 -------
// evita bloqueo pinza excesivamente abierta, tiende a CERRAR pinza
if ((valorPotX <= 57)and (pinAnalogico == 0))
{
}
// evita que el M5 choque con patilla Potenciometro 5 margen real 63 izda a 127 dcha
// aunque ponga 70º a 120º para tener mas garantía por las inercias.
if ((valorPotX < 70)and (pinAnalogico == 4))
{
analogWrite (pinPWM,180); // se podría quizas mejor poner antes StopMotores()***
}
//--------------------------------------------------------------------
} // fin de la funcion Joystick
// ----------------------------------------------------------
//-----------FUNCIÓN ir a Posicion HOME ---------------------
int Home (int PotMotorX,String motor,int pinPWM,int int1,int int2)

37
// ----------------------------------------------------------
{
int servoVal = analogRead (PotMotorX);
delayMicroseconds(100); // tiempo entre lecturas 0,1 ms
Serial.print (motor);
Serial.print (" ");
int valorPWM = 180; //valor Global para los motores salvo para el 1 y 2
if (motor == "motor 1")
{
valorPWM = 80;
}
if (motor == "motor 2")
{
valorPWM = 150;
}
while (1)
{
servoVal = analogRead (PotMotorX);
delayMicroseconds(100);
//------------------------
Serial.print (motor);
Serial.print (" ");
//------------------------
if ((servoVal>= 89)and (servoVal <= 91)) //Como veo q esta muy estable pondre 89 a 91
{
delay(200);
//de nuevo analizar y actuar en consonancia:
//si está entre 89 a 91 salir de lo contrario continuar

38
}
if ((servoVal >= 89)and (servoVal <= 91))
{
break; //salir
}
if (servoVal < 83) // velocidad giro grande
{
}
if ((servoVal >= 83) and (servoVal < 89) and (motor == "motor 1")) // REDUCIR VELOCIDAD
{
}
if ((servoVal >= 83) and (servoVal < 89) and (motor == "motor 2")) // REDUCIR VELOCIDAD ****
{
}
if ((servoVal >= 83) and (servoVal < 89) and (motor != "motor 1")and (motor != "motor 2")) // REDUCIR
VELOCIDAD
{
}
if ((servoVal > 95))
{
analogWrite (pinPWM,valorPWM); //velocidad giro grande
}
if ((servoVal >= 92)and(servoVal <= 95) and (motor == "motor 1"))
{

39
}
if ((servoVal >= 92)and(servoVal <= 95) and (motor == "motor 2"))
{
}
if ((servoVal >= 92) and (servoVal <= 95)and (motor != "motor 1")and (motor != "motor 2"))
{
}
}//fin del while
} // fin de la funcion Home
//---------------------------------------------------------
//-------Función de Stop Paro de Todos los motors----------
//---------------------------------------------------------
void StopMotores ()
{
int i = 0;
digitalWrite (2,LOW);// PONE A 0 las señales PWM de los enables motores
digitalWrite (3,LOW);
for (i=0; i<10; i++)
{
digitalWrite((22+i),LOW); // Pone a cero de la salida 22 a la 31
} // fin del for (i=0; i<10; i ++)
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 89º a 91º-------------
void Mensaje_Retorno_Posicion()
//-----------------------------------------------------
{
ledRojo(); //activar el led rojo mientras se va a Home, tambien se mantiene
//en la Secuenciar_movimientos()
Serial.println ("El brazo ira a posicion inicial 90 grados");
Serial.println ("despues de 5 segundos mover Joystick");
delay (5000); //tiempo para comenzar a utilizar el Joystick o efecturar el secuenciador
//------Desplazar los ejes a "Home (.......) ------------
///int Home (int PotMotorX,String motor,int pinPWM,int int1,int int2)

40
Home (9,"motor 5",6,30,31); //Preferible en primer lugar el motor 5 giro transversal
// se evita que si el M4 esta muy bajo y el giro del 5 es grande
//choque el la columna del M4 con el potenciometro del M5
Home (5,"motor 1",2,22,23); //Motor 1
Home (6,"motor 2",3,24,25); //Motor 2
Home (7,"motor 3",4,26,27); //Motor 3
Home (8,"motor 4",5,28,29); //Motor 4
//-------------------------------------------------------
} // fin void Mensaje_Retorno_Posicion()
//------FUNCIÓN para GRABAR EN SD las posicones de los SERVOS ------
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 no se ven
//luego escritos en la SD mas que 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);
miarchivo.println(); //el 13 y el 10 retorno y cambio linea finalizan la copia de cada posición
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 están declaradas como globales

41
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 está guardado 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 línea
//que es lo que separa cada posició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 ((miarchivo.read()== 13)and (miarchivo.read()== 10))
//Al llegar al cambio de línea activar el motor
if (c == 'A')//Cuando sea cambio linea
{
Serial.print("Secuenciar MOTOR 1 ");
Serial.print(valorInt); //imprimir en el Serial para ver los valores
Serial.println('A');
//int ActivarMotores ( int potMotorX,int valorInt,int pinPWM,int int1, int int2)
ActivarMotores(5,valorInt,2,22,23); // para activar el Motor 1
valorString = "" ; //iniciar la variable.

42
// NOTA no es necesario reiniciar "valorInt" al haber reiniciado: valorString = "" ;
} //fin del if (c == 'A')
if (c == 'B')//Cuando sea cambio linea
{
Serial.println('B');
valorString = "" ;
} //fin del if (c == 'B')
if (c == 'C')//Cuando sea cambio linea
{
Serial.println('C');
valorString = "" ;
} //fin del if (c == 'C')
if (c == 'D')//Cuando sea cambio linea
{
Serial.println('D');
valorString = "" ;
} //fin del if (c == 'D')
if (c == 'E')//Cuando sea cambio linea
{
Serial.println('E');
valorString = "" ;

43
} //fin del if (c == 'E')
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 funcion 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 ----------
int ActivarMotores (int PotMotorX,int valorInt,int pinPWM,int int1,int int2)
//-----------------------------------------------------------------
{
int valorPWM = 200; // para el resto de actuaciones a velocidad de 200
if (PotMotorX == 5) // se trata del potenciómetro del motor 1 pinzas
{
valorPWM = 80;
}
if (PotMotorX == 6) // se trata del potenciómetro del motor 2
{
valorPWM = 170;
}
if (PotMotorX == 9) // se trata del potenciómetro del motor 5
{
valorPWM = 200; // "lo podría suprimir se utilizaría el valor global valorPWM = 200"
}
//lo utilizo para que no se repitan ciclos innecesarios
int valorAnterior = 0 ;
while (1)
{
//1º se leen la posicion del potenciómetro de los motores
//para comparar después con el que debe tener

44
if (servoVal != valorAnterior) //para evitar repetir ciclos
{
//------------------------
Serial.print ("motor: ");
Serial.print (PotMotorX); //motor 0 a 4
Serial.print ("--");
//------------------------
if (abs(servoVal - valorInt) < 2) // Para dar una histéresis de +-1º
{
//// stopMotores(); //podría quizas emplearlo pero prefiero hacerlo así
// 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) // Para dar una histéresis de +-1º
{
//StopMotores();
break; //salir
}
if (servoVal < (valorInt-7))
{
}
//PARA REDUCIR VELOCIDAD AL M 1
if (servoVal >= (valorInt-7)and (servoVal< valorInt)and (PotMotorX == 5))

45
{
}
//el resto de motores a velocidad 110
if (servoVal >= (valorInt-7)and (servoVal< valorInt)and (PotMotorX != 5))
{
}
if (servoVal > (valorInt+7)) // si es MAYOR HIGH/LOW
{
}
// El PotMotorX == 5 es el Motor 1
if (servoVal <= (valorInt+7)and (servoVal >= (valorInt+1)and (PotMotorX == 5))) // +1 para compensación
{ //se puede suprimir el +1
}
//Resto de los Motores a velocidad de 110
if (servoVal <= (valorInt+7)and (servoVal >= (valorInt+1)and (PotMotorX != 5))) // +1 para compensacion
{ //se puede suprimir el +1
}
} // Fin del if (servoVal != valorAnterior)
valorAnterior = servoVal;
}//fin del while
} // fin de la funcion Activar movimientos
//-------------------------------
void setup() {
//---------------------------------
// pin 7 y 8 como entradas
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

46
pinMode(rojo,OUTPUT); //LED rojo pin 9 salida
pinMode(verde,OUTPUT); //LED verde pin 10 salida
pinMode(azul,OUTPUT); //LED azul pin 11 salida
// Control L298N del Motor 1
pinMode(ENA_1, OUTPUT); //pin 2 señal PWM
pinMode(IN1_1, OUTPUT); //pin 22
//inicio con todos apagados
apagarLeds(); //nota tengo en el pin 9 el rojo 10 el verde
//y 11 el azul
// -----POSICION INICIAL MOTORES DESCONECTADOS-------
digitalWrite (ENA_1,LOW) ; //Desconectar motor1
digitalWrite (IN1_1,LOW);

47
//----------------------------------------------------
//Inicializamos la conexión serial
Serial.begin(9600);
//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(10,OUTPUT);
pinMode(53,OUTPUT);//Necesario para la librería SD en Arduino Mega 2560
Serial.print("Initializing 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
{
delay(5); //Buble infinito hasta que se inserte tarjeta SD
}
} //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 void setup()
//---------------------------------------------------------
void loop()
//---------------------------------------------------------
{
// mensaje y desplazamiento a home
Mensaje_Retorno_Posicion ();
//--- AQUÍ ESTARÍAN LAS INSTRUCCIONES PARA hacer pruebas---
// con los Joystic y los potenciómetros.
/*
*/
//***las tengo al final del programa por si me hicieran
// de nuevo falta con los nº 2 y 3 ******
//----------------------------------------------------------
while (1) // Bucle infinito.
{
// pin analogico lectura Joystick + pin PWM activacion driver + pin giro + pin giro (dcha/izqda)

48
Joystick(0,2,22,23,'A'); //motor 1 (pinza)le asigno A al motor 1
Joystick(1,3,24,25,'B'); //motor 2 le asigno B al motor 2
Joystick(2,4,26,27,'C'); //motor 3 le asigno C al motor 3
Joystick(3,5,28,29,'D'); //motor 4 le asigno D al motor 4
Joystick(4,6,30,31,'E'); //motor 5 le asigno E al motor 5
} //fin del While (1)
} //fin del void loop()
// ---------- F I N D E L P R O G R A M A -----------------
// AQUÍ PONGO INSTRUCCIONES QUE ME HAN AYUDADO EN EL PROGRAMA.
/*
//1ª /* // IRÍA COLOCADO AL COMIENZO DE LA FUNCIÓN int Joystic (....)
//--------PARA DETECTAR PROBLEMA EN LECTURA INTERRUPTORES---
while (1) // para comprobar si hay ruido. era debido al doble parentesis que tenian los if
//mal if (digitalRead((Interruptor_grabar) == LOW))este provocaba que se activase tanto por low como por
high sin mas.
//mal if (digitalRead(Interruptor_grabar == LOW))
// bien if (digitalRead(Interruptor_grabar) == LOW)
{
if (digitalRead(Interruptor_grabar) == LOW)
{
Serial.println("pin 8 a cero Grabar_movimientos");
delay(1000);
}
if (digitalRead(Interruptor_grabar) == HIGH)
{
Serial.println("pin 8 a HIGH");
delay(1000);
}
} //fin del while de prueba
*/
//----------------------------------------------------
// INSTRUCCIONESW 2º Y 3º colocadas aala comienzo de la funcion void Loop ()
/* // 2º PARA LOS JOYSTIC ""NO BORRAR para hacer pruebas""
while (1)
{
servoVal = analogRead (0);
servoVal = map(servoVal,0,1023,0,179);
Serial.print("JOYSTICK 1 ");
Serial.println(servoVal);
delay(3000);

49
delay(3000);
delay(3000);
delay(3000);
delay(3000);
}
*/
/*
// 3º PARA LOS POTENCIÓMETROS ""NO BORRAR para hacer pruebas""
while (1)
{
servoVal = analogRead (5); //
Serial.print("MOTOR 1 ");
delay(2000);
servoVal = analogRead (6);//
delay(2000);
delay(2000);
delay(2000);

50
delay(2000);
}
*/
TUTORIAL USO DRIVER DUAL L298 PARA MOTORES DC y paso a paso
ESTA ES LA MISMA PLACA QUE COMPRE EN ALIEXPRESSS.
http://electronilab.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 esta 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 Roturas de motor y paradas *
1 0 El motor gira hacia adelante
0 1 El motor gira hacia atrás
1 1 Roturas de motor y paradas *
* Para costa un motor a una parada más lento, aplique una señal de baja a la línea ENABLE1.
En la parte inferior se encuentran los pines de control del módulo, marcados como IN1, IN2, IN3 e IN4.

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

52
 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
Más información:
 Hoja de datos (datasheet)
 Código de ejemplo

Proyecto brazo robotico ksr10 enero marzo 2015 a 9 v-version_2

Empfohlen

Empfohlen

Weitere ähnliche Inhalte

Was ist angesagt?

Was ist angesagt? (17)

Ähnlich wie Proyecto brazo robotico ksr10 enero marzo 2015 a 9 v-version_2

Ähnlich wie Proyecto brazo robotico ksr10 enero marzo 2015 a 9 v-version_2 (20)

Mehr von joaquinin1

Mehr von joaquinin1 (19)

Kürzlich hochgeladen

Kürzlich hochgeladen (12)

Proyecto brazo robotico ksr10 enero marzo 2015 a 9 v-version_2