#include <AccelStepper.h>
#include <Bounce2.h>
#include <ESP32Servo.h>
#include <WiFi.h>
#include <WebServer.h>
#include <EEPROM.h>
// Pines motores
const int pasoX = 18, dirX = 19;
const int pasoY = 21, dirY = 22;
const int pasoZ = 23, dirZ = 5;
// Pines joystick
const int jX = 32, jY = 33, jZ = 34, jServo = 35;
// Pines botones joystick
const int pinCalibrate = 12;
const int pinGoHome = 13;
// Pines LED y Enable
const int pinEnable = 2, ledEnable = 4;
// Pines LEDs modo
const int ledManual = 17, ledWifi = 16, ledReset = 25;
// EEPROM
#define EEPROM_SIZE 12
#define ADDR_X 0
#define ADDR_Y 4
#define ADDR_Z 8
// Objetos
Bounce modoSwitch = Bounce();
Bounce botonCalibrar = Bounce();
Servo mioServo;
AccelStepper motoreX(AccelStepper::DRIVER, pasoX, dirX);
AccelStepper motoreY(AccelStepper::DRIVER, pasoY, dirY);
AccelStepper motoreZ(AccelStepper::DRIVER, pasoZ, dirZ);
long valX, valY, valZ;
bool wifiMode = false;
int servoPos = 90;
int deadZoneMinX = 1800, deadZoneMaxX = 2300;
int deadZoneMinY = 1800, deadZoneMaxY = 2300;
int deadZoneMinZ = 1800, deadZoneMaxZ = 2300;
const char* ssid = "Robot polar";
const char* password = "12345678";
IPAddress local_ip(192, 168, 8, 103);
WebServer server(80);
long homeX = 0, homeY = 0, homeZ = 0;
void handleRoot() {
String html = "<!DOCTYPE html><html><head><meta name='viewport' content='width=device-width, initial-scale=1.0'>";
html += "<style>body{font-family:sans-serif;background:#e9f5ff;text-align:center;padding:20px;}";
html += ".grid{display:grid;grid-template-columns:repeat(3,80px);grid-gap:10px;justify-content:center;}";
html += "button{height:60px;width:80px;font-size:24px;border:none;border-radius:10px;background:#007BFF;color:white;box-shadow:2px 2px 5px #999;cursor:pointer;}";
html += "button:active{background:#0056b3;}.label{margin:10px;font-weight:bold;}</style></head><body>";
html += "<h2>Control WiFi - Robot Polar</h2>";
html += "<div class='label'>Base giratoria (X)</div><div class='grid'>";
html += "<button onclick=\"location.href='/move?direction=leftX'\">⬅️</button>";
html += "<button onclick=\"location.href='/move?direction=stop'\">⏹</button>";
html += "<button onclick=\"location.href='/move?direction=rightX'\">➡️</button></div>";
html += "<div class='label'>Brazo radial (Y)</div><div class='grid'>";
html += "<div></div><button onclick=\"location.href='/move?direction=upY'\">⬆️</button><div></div>";
html += "<div></div><button onclick=\"location.href='/move?direction=downY'\">⬇️</button><div></div></div>";
html += "<div class='label'>Altura (Z)</div><div class='grid'>";
html += "<div></div><button onclick=\"location.href='/move?direction=zup'\">⬆️</button><div></div>";
html += "<div></div><button onclick=\"location.href='/move?direction=zdown'\">⬇️</button><div></div></div>";
html += "<div class='label'>Funciones</div><div class='grid'>";
html += "<button onclick=\"location.href='/move?direction=calibrate'\">🎯 Calibrar</button>";
html += "<button onclick=\"location.href='/move?direction=home'\">🏠 Home</button></div>";
html += "</body></html>";
server.send(200, "text/html", html);
}
void handleMove() {
String dir = server.arg("direction");
if (dir == "leftX") motoreX.move(100);
else if (dir == "rightX") motoreX.move(-100);
else if (dir == "upY") motoreY.move(100);
else if (dir == "downY") motoreY.move(-100);
else if (dir == "zup") motoreZ.move(100);
else if (dir == "zdown") motoreZ.move(-100);
else if (dir == "stop") {
motoreX.stop();
motoreY.stop();
motoreZ.stop();
}
else if (dir == "calibrate") calibrarZonaMuerta();
else if (dir == "home") {
motoreX.moveTo(homeX);
motoreY.moveTo(homeY);
motoreZ.moveTo(homeZ);
}
server.sendHeader("Location", "/");
server.send(303);
}
void calibrarZonaMuerta() {
long sx = 0, sy = 0, sz = 0;
for (int i = 0; i < 100; i++) {
sx += analogRead(jX);
sy += analogRead(jY);
sz += analogRead(jZ);
delay(5);
}
deadZoneMinX = sx / 100 - 200; deadZoneMaxX = sx / 100 + 200;
deadZoneMinY = sy / 100 - 200; deadZoneMaxY = sy / 100 + 200;
deadZoneMinZ = sz / 100 - 200; deadZoneMaxZ = sz / 100 + 200;
digitalWrite(ledReset, HIGH); delay(1000); digitalWrite(ledReset, LOW);
}
void setup() {
Serial.begin(9600);
pinMode(jX, INPUT); pinMode(jY, INPUT); pinMode(jZ, INPUT); pinMode(jServo, INPUT);
pinMode(pinEnable, OUTPUT); pinMode(ledEnable, OUTPUT);
pinMode(pinGoHome, INPUT_PULLUP); pinMode(pinCalibrate, INPUT_PULLUP); pinMode(15, INPUT_PULLUP);
pinMode(ledManual, OUTPUT); pinMode(ledWifi, OUTPUT); pinMode(ledReset, OUTPUT);
digitalWrite(ledManual, HIGH); digitalWrite(ledWifi, LOW); digitalWrite(ledReset, LOW);
modoSwitch.attach(15); modoSwitch.interval(10);
botonCalibrar.attach(pinCalibrate); botonCalibrar.interval(5);
motoreX.setMaxSpeed(800); motoreX.setAcceleration(300);
motoreY.setMaxSpeed(800); motoreY.setAcceleration(300);
motoreZ.setMaxSpeed(800); motoreZ.setAcceleration(300);
mioServo.attach(14);
EEPROM.begin(EEPROM_SIZE);
EEPROM.get(ADDR_X, homeX); motoreX.setCurrentPosition(homeX);
EEPROM.get(ADDR_Y, homeY); motoreY.setCurrentPosition(homeY);
EEPROM.get(ADDR_Z, homeZ); motoreZ.setCurrentPosition(homeZ);
WiFi.softAP(ssid, password);
WiFi.softAPConfig(local_ip, local_ip, IPAddress(255, 255, 255, 0));
server.on("/", handleRoot);
server.on("/move", handleMove);
server.begin();
Serial.println("Sistema listo. IP: " + WiFi.softAPIP().toString());
}
void loop() {
modoSwitch.update();
botonCalibrar.update();
server.handleClient();
motoreX.run();
motoreY.run();
motoreZ.run();
if (!wifiMode) {
valX = analogRead(jX);
valY = analogRead(jY);
valZ = analogRead(jZ);
if (valX < deadZoneMinX) motoreX.setSpeed(-400);
else if (valX > deadZoneMaxX) motoreX.setSpeed(400);
else motoreX.setSpeed(0);
if (valY < deadZoneMinY) motoreY.setSpeed(-400);
else if (valY > deadZoneMaxY) motoreY.setSpeed(400);
else motoreY.setSpeed(0);
if (valZ < deadZoneMinZ) motoreZ.setSpeed(-400);
else if (valZ > deadZoneMaxZ) motoreZ.setSpeed(400);
else motoreZ.setSpeed(0);
motoreX.runSpeed();
motoreY.runSpeed();
motoreZ.runSpeed();
}
if (digitalRead(pinGoHome) == LOW) {
motoreX.moveTo(homeX);
motoreY.moveTo(homeY);
motoreZ.moveTo(homeZ);
Serial.println("Volviendo a Home");
}
if (modoSwitch.fell()) {
wifiMode = !wifiMode;
digitalWrite(ledManual, !wifiMode);
digitalWrite(ledWifi, wifiMode);
Serial.println(wifiMode ? "WiFi Activado" : "Manual Activado");
}
if (botonCalibrar.fell()) calibrarZonaMuerta();
Serial.printf("%ld,%ld,%ld,%d\n", motoreX.currentPosition(), motoreY.currentPosition(), motoreZ.currentPosition(), servoPos);
}