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