#include <Arduino.h>
#include <WiFi.h>
#include <WebServer.h>
#include <WebSocketsServer.h>
// ===== Credenciales Wi-Fi (MODIFICAR) =====
const char* ssid = "TU_REDU_WIFI";
const char* password = "TU_CONTRASENA";
// ===== Servidores =====
WebServer server(80);
WebSocketsServer webSocket(81);
// ===== Pines de Hardware =====
const int IN1 = 13; const int IN2 = 14;
const int IN3 = 27; const int IN4 = 26;
const int ENA = 25; const int ENB = 33;
const int freqPWM = 1000;
const int resolucionPWM = 8;
const int velocidadBase = 180;
#define ENC_IZQ_C1 18
#define ENC_IZQ_C2 19
#define ENC_DER_C1 32
#define ENC_DER_C2 23
const int TRIG_IZQ = 5; const int ECHO_IZQ = 4;
const int TRIG_DER = 21; const int ECHO_DER = 22;
// ===== Variables Compartidas y de Control IoT =====
volatile long pulsosIzq = 0;
volatile long pulsosDer = 0;
const int pulsosPorVuelta = 200;
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
volatile float distanciaIzqActual = 999.0;
volatile float distanciaDerActual = 999.0;
// Variables controladas por WebSocket
float distanciaUmbral = 25.0; // Ya no es const, el usuario la modifica
bool exploracionActiva = false;
unsigned long ultimoEnvioWS = 0;
bool ejecutandoManiobraEvasion = false;
long pulsosEvasionObjetivo = 0;
// ===== Geometría y Máquina de Estados =====
const float diametroRueda = 4.5;
const float distanciaEntreRuedas = 18.0;
const float circunferenciaRueda = PI * diametroRueda;
enum Movimiento { ADELANTE, DERECHA, IZQUIERDA, DETENIDO };
struct Comando {
Movimiento tipo;
long pulsosObjetivo;
};
long calcularPulsosGiro(float grados) {
float arco = (PI * distanciaEntreRuedas) * (grados / 360.0);
return (long)((arco / circunferenciaRueda) * pulsosPorVuelta);
}
Comando rutina[] = {
{ADELANTE, (long)((20.0 / (PI * 4.5)) * 200)},
{DERECHA, calcularPulsosGiro(90.0)},
{ADELANTE, (long)((20.0 / (PI * 4.5)) * 200)},
{IZQUIERDA, calcularPulsosGiro(90.0)}
};
const int totalPasos = sizeof(rutina) / sizeof(rutina[0]);
int indiceRutina = 0;
// ===== PÁGINA WEB EMBEBIDA (HTML/JS/CSS) =====
const char webpage[] PROGMEM = R"rawliteral(
<!DOCTYPE html>
<html lang="es">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>Panel de Control - Robot</title>
<style>
body { font-family: 'Segoe UI', Tahoma, Geneva, Verdana, sans-serif; background-color: #1e1e2f; color: #fff; text-align: center; margin: 0; padding: 20px; }
.card { background: #2a2a40; padding: 20px; border-radius: 10px; margin: 20px auto; max-width: 600px; box-shadow: 0 4px 8px rgba(0,0,0,0.3); }
h1 { color: #00d2ff; }
.btn { padding: 15px 30px; font-size: 18px; border: none; border-radius: 5px; cursor: pointer; margin: 10px; color: white; font-weight: bold; }
.btn-start { background-color: #28a745; }
.btn-stop { background-color: #dc3545; }
.btn:active { transform: scale(0.95); }
.slider-container { margin: 20px 0; }
input[type=range] { width: 80%; }
.sensor-data { display: flex; justify-content: space-around; font-size: 24px; margin-top: 20px; }
.alert { background-color: #ffc107; color: #000; padding: 10px; border-radius: 5px; display: none; font-weight: bold; margin-top: 20px;}
.alert.active { display: block; animation: flash 1s infinite; }
@keyframes flash { 0% {background-color: #ffc107;} 50% {background-color: #ff9800;} 100% {background-color: #ffc107;} }
</style>
</head>
<body>
<h1>Control de Navegación</h1>
<div class="card">
<button class="btn btn-start" onclick="sendCommand('START')">INICIAR EXPLORACIÓN</button>
<button class="btn btn-stop" onclick="sendCommand('STOP')">DETENER</button>
<div class="slider-container">
<h3>Distancia de Evasión: <span id="distVal">25</span> cm</h3>
<input type="range" min="10" max="300" value="25" id="sliderDist" onchange="updateThreshold(this.value)">
</div>
<div id="obstacleAlert" class="alert">¡OBSTÁCULO DETECTADO! EVADIENDO...</div>
<div class="sensor-data">
<div>Sensor Izq: <br><span id="sensIzq" style="color:#00d2ff;">0.0</span> cm</div>
<div>Sensor Der: <br><span id="sensDer" style="color:#00d2ff;">0.0</span> cm</div>
</div>
</div>
<script>
var gateway = `ws://${window.location.hostname}:81/`;
var websocket;
function initWebSocket() {
websocket = new WebSocket(gateway);
websocket.onmessage = onMessage;
}
function onMessage(event) {
var data = JSON.parse(event.data);
document.getElementById('sensIzq').innerText = data.izq.toFixed(1);
document.getElementById('sensDer').innerText = data.der.toFixed(1);
var alertBox = document.getElementById('obstacleAlert');
if(data.evadiendo) {
alertBox.classList.add('active');
} else {
alertBox.classList.remove('active');
}
}
function sendCommand(cmd) {
websocket.send(cmd);
}
function updateThreshold(val) {
document.getElementById('distVal').innerText = val;
websocket.send('THRESH:' + val);
}
window.addEventListener('load', initWebSocket);
</script>
</body>
</html>
)rawliteral";
// ===== Tarea FreeRTOS (Lectura Asíncrona de Sensores) =====
void tareaSensoresUltrasonicos(void *pvParameters) {
pinMode(TRIG_IZQ, OUTPUT); pinMode(ECHO_IZQ, INPUT);
pinMode(TRIG_DER, OUTPUT); pinMode(ECHO_DER, INPUT);
const unsigned long timeoutUS = 20000;
while (true) {
digitalWrite(TRIG_IZQ, LOW); delayMicroseconds(2);
digitalWrite(TRIG_IZQ, HIGH); delayMicroseconds(10);
digitalWrite(TRIG_IZQ, LOW);
long duracionIzq = pulseIn(ECHO_IZQ, HIGH, timeoutUS);
distanciaIzqActual = (duracionIzq == 0) ? 999.0 : duracionIzq * 0.034 / 2.0;
vTaskDelay(pdMS_TO_TICKS(30));
digitalWrite(TRIG_DER, LOW); delayMicroseconds(2);
digitalWrite(TRIG_DER, HIGH); delayMicroseconds(10);
digitalWrite(TRIG_DER, LOW);
long duracionDer = pulseIn(ECHO_DER, HIGH, timeoutUS);
distanciaDerActual = (duracionDer == 0) ? 999.0 : duracionDer * 0.034 / 2.0;
vTaskDelay(pdMS_TO_TICKS(30));
}
}
// ===== Interrupciones =====
void IRAM_ATTR isrEncoderIzq() {
portENTER_CRITICAL_ISR(&mux);
if (digitalRead(ENC_IZQ_C2) == HIGH) pulsosIzq++; else pulsosIzq--;
portEXIT_CRITICAL_ISR(&mux);
}
void IRAM_ATTR isrEncoderDer() {
portENTER_CRITICAL_ISR(&mux);
if (digitalRead(ENC_DER_C2) == HIGH) pulsosDer++; else pulsosDer--;
portEXIT_CRITICAL_ISR(&mux);
}
void resetEncoders() {
portENTER_CRITICAL(&mux);
pulsosIzq = 0; pulsosDer = 0;
portEXIT_CRITICAL(&mux);
}
void aplicarMovimiento(Movimiento mov) {
switch(mov) {
case ADELANTE:
digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
break;
case DERECHA:
digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
break;
case IZQUIERDA:
digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
break;
case DETENIDO:
digitalWrite(IN1, HIGH); digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH); digitalWrite(IN4, HIGH);
ledcWrite(ENA, 0); ledcWrite(ENB, 0);
break;
}
}
// ===== Control de Eventos WebSocket =====
void eventoWebSocket(uint8_t num, WStype_t type, uint8_t * payload, size_t length) {
if (type == WStype_TEXT) {
String mensaje = (char*)payload;
if (mensaje == "START") {
exploracionActiva = true;
resetEncoders();
aplicarMovimiento(rutina[indiceRutina].tipo);
Serial.println("Exploracion INICIADA via Web");
}
else if (mensaje == "STOP") {
exploracionActiva = false;
ejecutandoManiobraEvasion = false;
aplicarMovimiento(DETENIDO);
Serial.println("Exploracion DETENIDA via Web");
}
else if (mensaje.startsWith("THRESH:")) {
String valor = mensaje.substring(7);
distanciaUmbral = valor.toFloat();
Serial.print("Nuevo umbral configurado: "); Serial.println(distanciaUmbral);
}
}
}
// ===== Motor Principal de Navegación (Núcleo 1) =====
void actualizarNavegacion() {
if (!exploracionActiva) {
return; // Si está detenido por la web, no hace nada
}
// 1. GESTIÓN DE LA MANIOBRA DE EVASIÓN EN CURSO
if (ejecutandoManiobraEvasion) {
long pIzq, pDer;
portENTER_CRITICAL(&mux); pIzq = abs(pulsosIzq); pDer = abs(pulsosDer); portEXIT_CRITICAL(&mux);
if (pIzq >= pulsosEvasionObjetivo || pDer >= pulsosEvasionObjetivo) {
ejecutandoManiobraEvasion = false;
resetEncoders();
aplicarMovimiento(rutina[indiceRutina].tipo);
} else {
ledcWrite(ENA, velocidadBase); ledcWrite(ENB, velocidadBase);
}
return;
}
// 2. DETECCIÓN REACTIVA
if (distanciaIzqActual < distanciaUmbral || distanciaDerActual < distanciaUmbral) {
ejecutandoManiobraEvasion = true;
pulsosEvasionObjetivo = calcularPulsosGiro(35.0);
if (distanciaIzqActual < distanciaUmbral && distanciaDerActual >= distanciaUmbral) {
aplicarMovimiento(DERECHA);
} else if (distanciaDerActual < distanciaUmbral && distanciaIzqActual >= distanciaUmbral) {
aplicarMovimiento(IZQUIERDA);
} else {
aplicarMovimiento(DERECHA);
}
resetEncoders();
return;
}
// 3. CAPA DELIBERATIVA (Exploración FSM)
Comando cmdActual = rutina[indiceRutina];
long pIzq, pDer;
portENTER_CRITICAL(&mux); pIzq = abs(pulsosIzq); pDer = abs(pulsosDer); portEXIT_CRITICAL(&mux);
if (pIzq >= cmdActual.pulsosObjetivo || pDer >= cmdActual.pulsosObjetivo) {
indiceRutina = (indiceRutina + 1) % totalPasos;
resetEncoders();
aplicarMovimiento(rutina[indiceRutina].tipo);
return;
}
if (cmdActual.tipo == ADELANTE) {
float Kp = 1.5;
long error = pIzq - pDer;
int ajuste = error * Kp;
ledcWrite(ENA, constrain(velocidadBase - ajuste, 0, 255));
ledcWrite(ENB, constrain(velocidadBase + ajuste, 0, 255));
} else {
ledcWrite(ENA, velocidadBase); ledcWrite(ENB, velocidadBase);
}
}
// ===== Configuración e Inicialización =====
void setup() {
Serial.begin(115200);
// Configuración de Hardware Motor
pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
ledcAttach(ENA, freqPWM, resolucionPWM); ledcAttach(ENB, freqPWM, resolucionPWM);
aplicarMovimiento(DETENIDO);
pinMode(ENC_IZQ_C1, INPUT_PULLUP); pinMode(ENC_IZQ_C2, INPUT_PULLUP);
pinMode(ENC_DER_C1, INPUT_PULLUP); pinMode(ENC_DER_C2, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ENC_IZQ_C1), isrEncoderIzq, RISING);
attachInterrupt(digitalPinToInterrupt(ENC_DER_C1), isrEncoderDer, RISING);
// Iniciar WiFi
WiFi.mode(WIFI_STA);
WiFi.begin(ssid, password);
Serial.print("Conectando a WiFi");
while (WiFi.status() != WL_CONNECTED) { delay(500); Serial.print("."); }
Serial.println("\nConectado! IP: "); Serial.println(WiFi.localIP());
// Iniciar Servidor Web
server.on("/", []() {
server.send(200, "text/html", webpage);
});
server.begin();
// Iniciar WebSockets
webSocket.begin();
webSocket.onEvent(eventoWebSocket);
// Lanzar Tarea de Sensores en Núcleo 0
xTaskCreatePinnedToCore(tareaSensoresUltrasonicos, "SensoresUS", 2048, NULL, 1, NULL, 0);
}
// ===== Loop Principal (Núcleo 1) =====
void loop() {
// 1. Atender peticiones HTTP
server.handleClient();
// 2. Procesar WebSockets
webSocket.loop();
// 3. Ejecutar Navegación
actualizarNavegacion();
// 4. Enviar Telemetría (No bloqueante, cada 150ms)
if (millis() - ultimoEnvioWS > 150) {
ultimoEnvioWS = millis();
// Crear JSON manual para mayor velocidad (evita overhead de ArduinoJson)
String json = "{\"izq\":";
json += (distanciaIzqActual == 999.0) ? "\"Fuera de rango\"" : String(distanciaIzqActual, 1);
json += ",\"der\":";
json += (distanciaDerActual == 999.0) ? "\"Fuera de rango\"" : String(distanciaDerActual, 1);
json += ",\"evadiendo\":";
json += ejecutandoManiobraEvasion ? "true" : "false";
json += "}";
webSocket.broadcastTXT(json);
}
}