/*GRUA BLUETOOT INDEPENDIENTE -SOLO BRAZO
* Esta version es solo para el brazo robot
* Sin embargo el programa esta listo para acoplar
* a un movil constituido por 2 motores amarilos y
* tres servos para el control del brazo.
* Un motor paso a paso para el giro de la base.
* Contando el carrito con sensor de ultrasonido para detectar
* obstaculos por delante del movil.
* Cuenta con bocina y luces
* Los movimientos de los servos estan limitados para
* evitar daños en el giro.
* Configuracion de giro por inversion de sentido para el carrito
* Ultimas actualizacion 08-11-19
*/
#include <Servo.h> // incluye libreria de Servo
Servo servo; // crea objeto brazo largo (PIN2)
Servo servo1; // crea objeto brazo corto (PIN14)
Servo servo2; // crea objeto brazo punta (PIN15)
int PULSOMIN = 750; // pulso minimo en microsegundos
int PULSOMAX = 2750; // pulso maximo en microsegundos
int anglargo = 90; //almacena valor del giro de servo brazo largo
int angcorto = 90; //almacena valor del giro de servo brazo corto
int angpunta = 90; //almacena valor del giro de servo brazo punta
int num; // variable para almacenar valor leido por monitor o Bluetooth
int IN1 = 4;
int IN2 = 3;
int ENA = 11; //PIN PWM para variar velocidad del motor 1
int IN3 = 6;
int IN4 = 5;
int ENB = 9; //PIN PWM para variar velocidad del motor 2
int trespuesta =200;// tiempo de detencion en posicion
int tmotadel = 200; //tiempo de espera reaccion motor principal
int veloc = 0; // nivel de velocidad de motor principal (3 niveles)
int luces = 0; // Estado inicial de luces 0 apagadas 1 encendidas
int bocina = 0; //Estado inicial de bocina 0 apagado 1 encendidas
int avance = 0; //Almacena estodo- 0 freno 1 adelante 2 atras
int TP=7; //Tiempo entre pasos
//ULTRA 1
long distancia;
long tiempo;
//Fin ULTRA 1
void setup()
{
pinMode(IN1,OUTPUT);//Salida para IN1 del modulo
pinMode(IN2,OUTPUT);//Salida para IN2 del modulo
pinMode(IN3,OUTPUT);//Salida para IN3 del modulo
pinMode(IN4,OUTPUT);//Salida para IN4 del modulo
pinMode(12, OUTPUT);// PIN BOCINA
pinMode(10, OUTPUT);// PIN LUCES
pinMode(2, OUTPUT);// PIN SERVO brazo largo
pinMode(14, OUTPUT); // configura ‘14’ como salida brazo corto
pinMode(15, OUTPUT); // configura ‘15’ como salida punta
//para motor paso a paso
pinMode(16, OUTPUT);
pinMode(17, OUTPUT);
pinMode(18, OUTPUT);
pinMode(19, OUTPUT);
digitalWrite(16,LOW);//Fija salida a bajo apagado
digitalWrite(17,LOW);//Fija salida a bajo apagado
digitalWrite(18,LOW);//Fija salida a bajo apagado
digitalWrite(19,LOW);//Fija salida a bajo apagado
digitalWrite(12,LOW);//Fija salida a bajo apagado
digitalWrite(10,LOW);//Fija salida a bajo apagado
//--ULTRA1---
pinMode(8, OUTPUT); /*activación del pin 8 como salida: para el pulso ultrasónico*/
pinMode(7, INPUT); /*activación del pin 7 como entrada: tiempo del rebote del ultrasonido*/
//----fin ULTRA1------
//---para los servos------
servo.attach(2, PULSOMIN, PULSOMAX); // inicializacion de servo
servo1.attach(14, PULSOMIN, PULSOMAX); // inicializacion de servo
servo2.attach(15, PULSOMIN, PULSOMAX); // inicializacion de servo
//---fin para los servos---------------
Serial.begin(9600);
//EStablese posicion inicial de los servos para acomodar grua
girarservo();
delay(200);
girarservo1();
delay(200);
girarservo2();
delay(200);
//------FIN posicion inicial de brazos---
}
void loop()
{
if(Serial.available()>=1)
{
//Delay para favorecer la lectura de caracteres
delay(22);// Cambiar para simulacion Proteus 400 -Original 22
//Se crea una variable que servirá como buffer
String bufferString = "";
/*
* Se le indica a Arduino que mientras haya datos
* disponibles para ser leÃdos en el puerto serie
* se mantenga concatenando los caracteres en la
* variable bufferString
*/
while (Serial.available()>0) {
bufferString += (char)Serial.read();
}
num = bufferString.toInt(); //Se carga lo leido en la variable entrada
/*//ACTIVAR SOLO PARA EL CONTROL DEL PROGRAMA
//-----Muestra angulos actuales de los servos
//ACTIVAR SOLO PARA CONTROL DEL PROGRAMADOR
Serial.print("Orden recibida =");
Serial.println(num);
Serial.print("Brazo largo =");
Serial.println(anglargo);
Serial.print("Brazo corto =");
Serial.println(angcorto);
Serial.print("Brazo punta =");
Serial.println(angpunta);
//--------------FIN CONTROL------------------------------
*/
analisisnum();
}// de Serial Available
//objetocerca(); //verifica objetos cerca
//versichoca(); // verifica si va a chocar- avisa y retrocede
}
//------------------Funcion analisisnum--------------------
void analisisnum()
{
//Serial.println(veloc);
if (num==700)// NO USADO -DISPONIBLE
{
//delay(trespuesta);
}
if (num==800) //Giro a la izquierda
{
girarizquierda();
// delay(trespuesta);
}
if (num==750) //Giro a al derecha
{
girarderecha();
//delay(trespuesta);
}
//---------------------Adelante--------------------------------
if (num==100) //Adelante
{
digitalWrite(IN1,HIGH); //IN1
digitalWrite(IN2,LOW); //IN2
digitalWrite(IN3,HIGH); //IN3
digitalWrite(IN4,LOW); //IN4
avance =1;
if(veloc == 0)
{
analogWrite(ENA,255);
analogWrite(ENB,255);
delay(tmotadel);
delay(tmotadel);
analogWrite(ENA,180);
analogWrite(ENB,180);
delay(tmotadel);
}
if(veloc == 1)
{
analogWrite(ENA,230);
analogWrite(ENB,230);
delay(tmotadel);
}
if(veloc == 2)
{
analogWrite(ENA,255);
analogWrite(ENB,255);
//Indica velocidad maxima
digitalWrite(12,HIGH);//bocina encendida
delay(50);
digitalWrite(12,LOW);//bocina apagada
delay(50);
//Indica velocidad maxima
delay(tmotadel);
}
veloc = veloc +1;
if(veloc > 2) veloc =2;
delay(tmotadel);
}
//-----------------Fin adelante------------------------------
if (num==200) //Atras
{
digitalWrite(IN1,LOW); //IN1
digitalWrite(IN2,HIGH); //IN2
digitalWrite(IN3,LOW); //IN3
digitalWrite(IN4,HIGH); //IN4
avance =2;
if(veloc == 0)
{
analogWrite(ENA,255);
analogWrite(ENB,255);
delay(tmotadel);
delay(tmotadel);
analogWrite(ENA,180);
analogWrite(ENB,180);
delay(tmotadel);
}
if(veloc == 1)
{
analogWrite(ENA,230);
analogWrite(ENB,230);
delay(tmotadel);
}
if(veloc == 2)
{
analogWrite(ENA,255);
analogWrite(ENB,255);
delay(tmotadel);
}
veloc = veloc +1;
if(veloc > 2) veloc =2;
delay(tmotadel);
}
//-----------------Fin atras----------------------------
//----------------Freno---------------------------------
if (num==300) //FRENO
{
num =666; //valor neutro
freno();
}
//----------------FIN Freno---------------------------------
//----------------LUCES---------------------------------
if (num==400) // encendido de luces
{
if(luces == 0)
{
digitalWrite(10,HIGH);//RELE LUCES encendidas
luces = 1; //Indica que luces estan encendidas
}
else
{
digitalWrite(10,LOW);//RELE LUCES apagado
luces = 0; //Indica que luces estan encendidas
}
}
//----------------FIN LUCES-------------------------------
//----------------BOCINA---------------------------------
if (num==500) // encendido de bocina
{
if(bocina == 0)
{
digitalWrite(12,HIGH);//bocina encendida
delay(150);
digitalWrite(12,LOW);//bocina apagada
delay(150);
digitalWrite(12,HIGH);//bocina encendida y queda
bocina = 1; //Indica que bocina esta encendida
}
else
{
digitalWrite(12,LOW);//bocina apagado
bocina = 0; //Indica que bocina esta encendida
}
}
//----------------FIN BOCINA-------------------------------
//--------------para brazo largo---------------------------
if (num == 210) // mover brazo largo horario
{
brazolargoh();
//delay(tmotadel);
}
if (num == 220) // mover brazo largo anti horario
{
brazolargoah();
//delay(tmotadel);
}
//--------------FIN para brazo largo---------------------------
//--------------para brazo corto---------------------------
if (num == 230) // mover brazo largo horario
{
brazocortoh();
//delay(tmotadel);
}
if (num == 240) // mover brazo largo antihorario
{
brazocortoah();
//delay(tmotadel);
}
//--------------FIN para brazo corto---------------------------
//--------------para brazo punta---------------------------
if (num == 250) // mover brazo punta horario
{
brazopunth();
// delay(tmotadel);
}
if (num == 260) // mover brazo punta anti horario
{
brazopuntah();
//delay(tmotadel);
}
//--------------FIN para brazo punta---------------------------
if (num == 265) // acomoda brazos para trabajar
{
acomoda();
//delay(tmotadel);
}
//-------------------------------------------------------------
if (num == 268) // acomoda brazos para trabajar
{
estaciona();
//delay(tmotadel);
}
//--------------------------------------------------------------
if (num == 310) // gira base horario
{
pasohorario();
//delay(tmotadel);
}
//--------------------------------------------------------------
if (num == 320) // gira base horario
{
pasoantihorario();
//delay(tmotadel);
}
//--------------------------------------------------------------
}//llave final de funcion analisisnum---
//Fin funcion analisisnum---------------------------
//--------------------Funcion Freno-----------------
void freno()
{
analogWrite(ENA,0);
analogWrite(ENB,0);
digitalWrite(IN1,LOW); //IN1
digitalWrite(IN2,LOW); //IN2
digitalWrite(IN3,LOW); //IN3
digitalWrite(IN4,LOW); //IN4
avance =0;
veloc = 0;
delay(tmotadel);
}
//--------------------fin Funcion Freno-----------------
//-----------Funcion girar derecha-------------------
void girarderecha()
{
digitalWrite(IN1,LOW); //IN1
digitalWrite(IN2,HIGH); //IN2
digitalWrite(IN3,HIGH); //IN3
digitalWrite(IN4,LOW); //IN4
analogWrite(ENA,170);
analogWrite(ENB,170);
delay(trespuesta);
}
//---Fin funcion girar a la derecha -----------------
//-----------Funcion girar izquierda-------------------
void girarizquierda()
{
digitalWrite(IN1,HIGH); //IN1
digitalWrite(IN2,LOW); //IN2
digitalWrite(IN3,LOW); //IN3
digitalWrite(IN4,HIGH); //IN4
analogWrite(ENA,170);
analogWrite(ENB,170);
delay(trespuesta);
}
//---Fin funcion girar a la izquierda----------------
//--Funcion objetocerca------------------------------
void objetocerca()
{
digitalWrite(8,LOW); /* Por cuestión de estabilización del sensor*/
delayMicroseconds(5);//ORIGINAL
digitalWrite(8, HIGH); /* envÃo del pulso ultrasónico*/
delayMicroseconds(10);//ORIGINAL
tiempo=pulseIn(7, HIGH); /* Función para medir la longitud del pulso entrante. Mide el tiempo que transcurrido entre el envÃo
del pulso ultrasónico y cuando el sensor recibe el rebote, es decir: desde que el pin 8 empieza a recibir el rebote, HIGH, hasta que
deja de hacerlo, LOW, la longitud del pulso entrante*/
distancia= int(0.017*tiempo); /*fórmula para calcular la distancia obteniendo un valor entero*/
/*Monitorización en centÃmetros por el monitor serial*/
// Serial.print("Distancia = ");
//Serial.print(distancia);
//Serial.println(" cm");
//delay(1000);// agregue esta instruccion si no logra medir
}
//---Fin Funcion objetocerca-------------------------
//---Funcion versichoca--------
void versichoca()
{
if (distancia < 10)
{
freno();
digitalWrite(10,HIGH);// LUCES encendidas
digitalWrite(12,HIGH);// bocina encendida
delay (100);
digitalWrite(12,LOW);//bocina encendida
delay(100);
digitalWrite(12,HIGH);//bocina encendida
delay (100);
digitalWrite(12,LOW);//bocina encendida
delay(100);
//retrocede un poco-----
digitalWrite(IN1,LOW); //IN1
digitalWrite(IN2,HIGH); //IN2
digitalWrite(IN3,LOW); //IN3
digitalWrite(IN4,HIGH); //IN4
analogWrite(ENA,255);
analogWrite(ENB,255);
delay(200);
freno();
digitalWrite(10,LOW);// LUCES apagadas
}// de distancia < que 10- distancia para evitar choque
}
//---FIN Funcion versichoca--------
//---brazolargo--------------------
void brazolargoh()
{
anglargo = anglargo + 10;
if(anglargo > 140)
{
anglargo = anglargo - 10;
digitalWrite(12, HIGH);
delay(100);
digitalWrite(12, LOW);
}
girarservo();
}
void brazolargoah()
{
anglargo = anglargo - 10;
if(anglargo < 60)
{
anglargo = anglargo + 10;
digitalWrite(12, HIGH);
delay(100);
digitalWrite(12, LOW);
}
girarservo();
}
//---FIN brazolargo--------------------
//---brazocorto--------------------
void brazocortoh()
{
angcorto = angcorto - 10;
if(angcorto < 10)
{
angcorto = angcorto + 10;
digitalWrite(12, HIGH);
delay(100);
digitalWrite(12, LOW);
}
girarservo1();
}
void brazocortoah()
{
angcorto = angcorto + 10;
if(angcorto > 150)
{
angcorto = angcorto - 10;
digitalWrite(12, HIGH);
delay(100);
digitalWrite(12, LOW);
}
girarservo1();
}
//---FIN brazocorto--------------------
//---brazopunta--------------------
void brazopunth()
{
angpunta = angpunta + 10;
if(angpunta > 180)
{
angpunta = angpunta - 10;
digitalWrite(12, HIGH);
delay(100);
digitalWrite(12, LOW);
}
girarservo2();
}
void brazopuntah()
{
angpunta = angpunta - 10;
if(angpunta < 10)
{
angpunta = angpunta + 10;
digitalWrite(12, HIGH);
delay(100);
digitalWrite(12, LOW);
}
girarservo2();
}
//---FIN brazopunta--------------------
//-----------Funcion girar servo-------------------
void girarservo()
{
servo.write(anglargo); // envia al servo el valor del angulo
delay(20); // demora para que el servo llegue a posicion
}
//-----------Funcion girar servo1-------------------
void girarservo1()
{
servo1.write(angcorto); // envia al servo el valor del angulo
delay(20); // demora para que el servo llegue a posicion
}
//-----------Funcion girar servo2-------------------
void girarservo2()
{
servo2.write(angpunta); // envia al servo el valor del angulo
delay(20); // demora para que el servo llegue a posicion
}
//-----FIN gira servo 3 MOTORES---------------------
//---------------------acomoda brazos------
void acomoda()
{
anglargo = 90; //brazo largo
angcorto = 90; //brazo corto
angpunta = 90; //brazo punta
girarservo();
delay(200);
girarservo1();
delay(200);
girarservo2();
delay(200);
}
//-------------fin acomoda brazos------------
//-----Estaciona brazos para guardar--------
void estaciona()
{
anglargo = 70; //brazo largo
angcorto = 140; //brazo corto
angpunta = 90; //brazo punta
girarservo();
delay(200);
girarservo1();
delay(200);
girarservo2();
delay(200);
}
//-----Estaciona brazos para guardar--------
//-------Giro horario motor paso a paso------
void pasohorario()
{
for (int i=0; i<10; i++) //
{
digitalWrite(19, HIGH); // enciende el pin
delay(TP); // espera
digitalWrite(19, LOW); // apaga el pin
digitalWrite(18, HIGH);// enciende el pin
delay(TP); // espera
digitalWrite(18, LOW); // apaga el pin
digitalWrite(17, HIGH); // enciende el pin
delay(TP); // espera
digitalWrite(17, LOW); // apaga el pin
digitalWrite(16, HIGH); // Enciende el pin
delay(TP); // espera
digitalWrite(16, LOW); // apaga el pin
}
}
//-------Fin Giro horario motor paso a paso------
//-------Giro antihorario motor paso a paso------
void pasoantihorario()
{
for (int i=0; i<10; i++) //
{
digitalWrite(16, HIGH); // enciende el pin
delay(TP); // espera
digitalWrite(16, LOW); // apaga el pin
digitalWrite(17, HIGH);// enciende el pin
delay(TP); // espera
digitalWrite(17, LOW); // apaga el pin
digitalWrite(18, HIGH); // enciende el pin
delay(TP); // espera
digitalWrite(18, LOW); // apaga el pin
digitalWrite(19, HIGH); // Enciende el pin
delay(TP); // espera
digitalWrite(19, LOW); // apaga el pin
}
}
//-------Fin Giro antihorario motor paso a paso------