#include <ESP32Servo.h>
//pines del puente H
//Ruedas delanteras
int entrada1 = 27;
int entrada2 = 26;
//Ruedas traseras
int entrada3 = 25;
int entrada4 = 33;
// Pines del sensor de ultrasonido
const int echoPin = 23;
const int trigPin = 22;
// Pines de los servos
const int servoPin1 = 19;
const int servoPin2 = 18;
// Variables para el sensor de ultrasonido
float duracion, cm;
float vel_sonido = 340.0;
// Crear objetos servo
Servo myservo1;
Servo myservo2;
// Distancia mínima para detener los servos (en cm)
const int distanciaDeteccion = 10;
// Variables para el temporizador
unsigned long tiempoUltimaDeteccion = 0;
unsigned long tiempoRueda = 0;
unsigned long tiempoActualRueda = 0;
const unsigned long tiempoEsperaDeteccion = 1000; // 1 segundo en milisegundos
const unsigned long tiempoMovimiento = 10000; // 3 segundos en milisegundos
const unsigned long tiempoRetrasoSegundoServo = 1000; // 1 segundo en milisegundos
const unsigned long tiempoAdelanteActivo = 500;
bool objetoDetectado = false;
bool moviendoA150 = false;
bool moviendoA50 = false;
bool ifServo2 = false;
bool AdelanteRecoger = false;
bool RetrocederRecoger = false;
unsigned long tiempoInicioMovimiento = 0;
unsigned long tiempoInicioSegundoServo = 0;
const unsigned long tiempoCambioPosicion2 = 3000; // Tiempo en milisegundos que tarda el segundo servo en volver a la posición inicial
// Variables para almacenar la posición de los servos
int servoPos1 = 150; // Posición inicial del primer servo
int servoPos2 = 80; // Posición inicial del segundo servo
int direction = 2; // Dirección del movimiento: 1 (incrementar), -1 (decrementar)
const int limiteSuperior = 160;
const int limiteInferior = 50;
const int limiteRecogerR = 104;
const int limiteRecogerL = 94;
int servo1Info = 0;
void setup() {
Serial.begin(9600); // Iniciar comunicación serie
//Pines Puente H
pinMode(entrada1, OUTPUT);
pinMode(entrada2, OUTPUT);
pinMode(entrada3, OUTPUT);
pinMode(entrada4, OUTPUT);
// Configurar pines del sensor de ultrasonido
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
// Adjuntar los servos a los pines especificados
myservo1.attach(servoPin1);
myservo2.attach(servoPin2);
// Posiciones iniciales de los servos
myservo1.write(servoPos1);
myservo2.write(servoPos2);
}
void loop() {
// Enviar pulso al sensor de ultrasonido
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Leer la duración del pulso de retorno
duracion = pulseIn(echoPin, HIGH);
// Calcular la distancia en cm
cm = (duracion / 2) * (vel_sonido / 10000);
// Imprimir la distancia en el monitor serie
// Serial.print(cm, 0);
// Serial.println(" cm");
// Comprobar si se detecta un objeto
if (cm <= distanciaDeteccion) {
servo1Info = myservo1.read();
Serial.println(servo1Info);
if (servo1Info < limiteRecogerR && servo1Info > limiteRecogerL && !objetoDetectado) {
objetoDetectado = true;
tiempoUltimaDeteccion = millis(); // Marcar el tiempo de detección
Serial.println("Objeto detectado.");
}
}
if (objetoDetectado) {
unsigned long tiempoActual = millis();
// Esperar 1 segundo después de la detección
if (tiempoActual - tiempoUltimaDeteccion < tiempoEsperaDeteccion) {
// No hacer nada durante la espera
Serial.println( objetoDetectado);
} else if (!moviendoA150) {
// Mover el primer servo a 150 grados y esperar 3 segundos
moviendoA150 = true;
tiempoInicioMovimiento = tiempoActual;
myservo1.write(150); // Mover el primer servo a 150 grados
Serial.println("Primer servo movido a 150 grados.");
ifServo2 = true;
// Activar el segundo servo un segundo después
} else if (tiempoActual - tiempoInicioMovimiento < tiempoMovimiento) {
// Esperar 3 segundos mientras el primer servo está en 150 grados
} else {
// Reiniciar el ciclo de movimiento del primer servo desde 150 grados
moviendoA150 = false;
servoPos1 = 150; // Reiniciar la posición inicial del primer servo
myservo1.write(servoPos1);
Serial.println("Inicio del ciclo de movimiento del primer servo desde 150 grados.");
// Mover el segundo servo de vuelta a 90 grados después de 3 segundos
if (moviendoA50 && tiempoActual - tiempoInicioSegundoServo >= tiempoCambioPosicion2) {
myservo2.write(90);
moviendoA50 = false;
Serial.println("Segundo servo movido de vuelta a 90 grados.");
}
objetoDetectado = false; // Permitir nueva detección
}
if ( ifServo2 == true && tiempoActual - tiempoInicioMovimiento >= tiempoRetrasoSegundoServo) {
moviendoA50 = true;
myservo2.write(5); // Mover el segundo servo a 50 grados
Serial.println("Segundo servo movido a 50 grados.");
ifServo2 = false;
AdelanteRecoger = true;
}
} else {
// Si no hay objeto detectado, mover el primer servo motor
servoPos1 += direction;
if (servoPos1 >= limiteSuperior || servoPos1 <= limiteInferior) {
direction = -direction; // Cambiar dirección al llegar a los límites
}
myservo1.write(servoPos1);
delay(5); // Pausa corta para permitir que el primer servo se mueva rápidamente
// Mover el segundo servo en un ciclo diferente si es necesario
// Ejemplo: puede agregar lógica aquí para el segundo servo si no se está moviendo en respuesta a la detección
}
tiempoActualRueda = millis();
if (!AdelanteRecoger){
tiempoRueda = tiempoActualRueda;
}else {
if(tiempoActualRueda > tiempoRueda + tiempoAdelanteActivo){
if (tiempoActualRueda > tiempoRueda + tiempoAdelanteActivo + tiempoAdelanteActivo){
digitalWrite(entrada3, LOW);
digitalWrite(entrada4, LOW);
AdelanteRecoger = false;
}else{
digitalWrite(entrada3, LOW); // Activar entrada3
digitalWrite(entrada4, HIGH); // Desactivar entrada4
}
}
}
// Pausa antes de la siguiente medición
delay(100);
}
//#include <Wire.h>
// #include "Adafruit_TCS34725.h"
// // Inicializar el sensor con tiempo de integración de 600ms y ganancia de 1x
// Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_600MS, TCS34725_GAIN_1X);
// void setup(void) {
// // Iniciar la comunicación serie a 9600 bps
// Serial.begin(9600);
// // Configurar los pines I2C para ESP32
// Wire.begin(21, 22);
// // Intentar inicializar el sensor
// if (tcs.begin()) {
// Serial.println("Found sensor");
// } else {
// Serial.println("No TCS3472 found ... check your connections");
// while (1); // Bucle infinito si no se encuentra el sensor
// }
// }
// void loop(void) {
// uint16_t r, g, b, c;
// tcs.getRawData(&r, &g, &b, &c);
// // Imprimir los valores en bruto del sensor
// Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
// Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
// Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
// Serial.print("C: "); Serial.print(c, DEC); Serial.println(" ");
// // Determinar el color del objeto
// String color = identifyColor(r, g, b);
// // Imprimir el color identificado
// Serial.print("Color identificado: ");
// Serial.println(color);
// delay(1000); // Esperar un segundo antes de la siguiente lectura
// }
// // Función para identificar el color del objeto
// String identifyColor(uint16_t r, uint16_t g, uint16_t b) {
// if (r > 100 && g > 100 && b < 1400) { // Ajustar los umbrales según sea necesario
// return "Amarillo";
// } else if (g > r && g > b) {
// return "Verde";
// } else if (b > r && b > g) {
// return "Azul";
// } else if (r > g && r > b) {
// return "Rojo";
// } else {
// return "Color desconocido";
// }
// }
//Codigo color con servo
// #include <Wire.h>
// #include "Adafruit_TCS34725.h"
// #include <ESP32Servo.h>
// // Inicializar el sensor con tiempo de integración de 600ms y ganancia de 1x
// Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_600MS, TCS34725_GAIN_1X);
// // Crear un objeto Servo
// Servo myservo;
// // Variables para almacenar el color detectado y el tiempo
// String detectedColor = "";
// String lastDetectedColor = "";
// unsigned long colorDetectedTime = 0;
// unsigned long restStartTime = 0; // Tiempo cuando comienza el descanso
// bool colorDetected = false;
// bool resting = false; // Indicador de si el sensor está en descanso
// void setup(void) {
// // Iniciar la comunicación serie a 9600 bps
// Serial.begin(9600);
// // Configurar los pines I2C para ESP32
// Wire.begin(21, 22);
// // Inicializar el servo en el pin 16
// myservo.attach(16);
// // Intentar inicializar el sensor
// if (tcs.begin()) {
// Serial.println("Found sensor");
// } else {
// Serial.println("No TCS3472 found ... check your connections");
// while (1); // Bucle infinito si no se encuentra el sensor
// }
// // Posicionar el servo en la posición inicial (0 grados)
// myservo.write(0);
// }
// void loop(void) {
// unsigned long currentTime = millis();
// if (resting) {
// // Si está en descanso, comprobar si ha pasado el tiempo de descanso
// if (currentTime - restStartTime >= 3000) {
// resting = false; // Terminar el descanso
// Serial.println("Sensor is now active.");
// } else {
// return; // Salir de la función loop mientras está en descanso
// }
// }
// uint16_t r, g, b, c;
// tcs.getRawData(&r, &g, &b, &c);
// // Imprimir los valores en bruto del sensor
// Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
// Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
// Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
// Serial.print("C: "); Serial.print(c, DEC); Serial.println(" ");
// // Determinar el color del objeto
// String color = identifyColor(r, g, b);
// // Imprimir el color identificado
// Serial.print("Color identificado: ");
// Serial.println(color);
// // Si se detecta un nuevo color
// if (color != "Color desconocido" && color != detectedColor) {
// detectedColor = color;
// colorDetectedTime = currentTime;
// colorDetected = true;
// myservo.write(90); // Mover el servo a 50 grados
// Serial.println("Color detected and servo moved to 50 degrees");
// }
// // Si el color se ha detectado durante 3 segundos
// if (colorDetected && (currentTime - colorDetectedTime >= 3000)) {
// // Leer nuevamente el color
// tcs.getRawData(&r, &g, &b, &c);
// color = identifyColor(r, g, b);
// // Si se detecta el mismo color nuevamente
// if (color == detectedColor) {
// myservo.write(0); // Mover el servo a 0 grados
// Serial.println("Same color detected again, servo moved back to 0 degrees");
// lastDetectedColor = detectedColor; // Guardar el color detectado
// detectedColor = ""; // Reiniciar el color detectado
// colorDetected = false; // Reiniciar la detección de color
// resting = true; // Activar el descanso del sensor
// restStartTime = currentTime; // Marcar el inicio del descanso
// } else {
// // Si no es el mismo color, resetear el tiempo y continuar detectando
// colorDetectedTime = currentTime;
// }
// }
// // El tiempo de descanso ya se ha manejado, así que solo esperar un poco antes de la siguiente lectura
// delay(100);
// }
// // Función para identificar el color del objeto
// String identifyColor(uint16_t r, uint16_t g, uint16_t b) {
// if (r >= 140 && r <= 190 && g >= 210 && g <= 250 && b >= 105 && b <= 180) {
// return "Amarillo";
// } else if (r >= 40 && r <= 58 && g >= 90 && g <= 120 && b >= 75) {
// return "Verde";
// } else if (r >= 50 && r <= 140 && g >= 100 && g <= 290 && b >= 110 && b <= 500) {
// return "Azul";
// } else if (r >= 100 && r <= 120 && g >= 85 && g <= 105 && b >= 75 && b <= 95) {
// return "Rojo";
// } else {
// return "Color desconocido";
// }
// }
//(r + 100) >= g && (r - 100) > 500 && b < 1400)
// https://www.youtube.com/watch?v=OPEYwuAbLBI
// https://www.youtube.com/watch?v=-qf8MpPAcIs tutorial sensor de Color
//Codigo sensor de color tcs3472
// #include <Wire.h>
// #include "Adafruit_TCS34725.h"
// /*
// Conexiones:
// - SCL a GPIO 22
// - SDA a GPIO 21
// - VDD a 3.3V DC
// - GND a tierra común
// */
// #include "BluetoothSerial.h"
// String device_name = "ESP32-BT-Slave";
// #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
// #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
// #endif
// #include <ESP32Servo.h>
// #include <bits/c++config.h>
// #if !defined(CONFIG_BT_SPP_ENABLED)
// #error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
// #endif
// Servo myservo;
// int pos = 5;
// BluetoothSerial SerialBT;
// int entrada1 = 27;
// int entrada2 = 26;
// int entrada3 = 25;
// int entrada4 = 33;
// void setup() {
// myservo.attach(18);
// Serial.begin(115200);
// SerialBT.begin(device_name);
// Serial.printf("The device with name \"%s\" is started.\nNow you can pair it with Bluetooth!\n", device_name.c_str());
// pinMode(entrada1, OUTPUT);
// pinMode(entrada2, OUTPUT);
// pinMode(entrada3, OUTPUT);
// pinMode(entrada4, OUTPUT);
// myservo.write(pos);
// }
// void loop() {
// char dato = SerialBT.read();
// if (dato == 'T'){
// pos = min(pos + 10, 180);
// myservo.write(pos);
// };
// if (dato == 'X'){
// pos = max(pos - 10, 5);
// myservo.write(pos);
// };
// if (dato == 'C'){
// digitalWrite(entrada1, LOW);
// digitalWrite(entrada2, HIGH);
// };
// if (dato == 'S'){
// digitalWrite(entrada1, HIGH);
// digitalWrite(entrada2, LOW);
// };
// if (dato == 'F'){
// digitalWrite(entrada3, LOW);
// digitalWrite(entrada4, HIGH);
// };
// if (dato == 'B'){
// digitalWrite(entrada3, HIGH);
// digitalWrite(entrada4, LOW);
// };
// if (dato == '0'){
// digitalWrite(entrada1, LOW);
// digitalWrite(entrada2, LOW);
// digitalWrite(entrada3, LOW);
// digitalWrite(entrada4, LOW);
// };
// if (dato == '1'){
// digitalWrite(entrada3, LOW);
// digitalWrite(entrada4, LOW);
// };
// // if (Serial.available()) {
// // SerialBT.write(Serial.read());
// // }
// // if (SerialBT.available()) {
// // Serial.write(SerialBT.read());
// // }
// delay(20);
// }