From 19dc4bcf90935a9af04e4c91b4f5291ab37fff7b Mon Sep 17 00:00:00 2001 From: Juan Date: Thu, 21 May 2026 09:51:42 -0500 Subject: [PATCH] Funciona todo sin modulo_imu --- AutoCalibracion/AutoCalibracion.ino | 395 ++++++++++++++++++++++++++++ a.out | Bin 0 -> 76064 bytes main.py | 208 +++++++++++++++ modulo_camara.py | 29 ++ modulo_esp32.py | 192 ++++++++++++++ modulo_imu.py | 296 +++++++++++++++++++++ modulo_slam.py | 196 ++++++++++++++ out2.txt | 0 output_test.txt | 3 + planificador_astar.py | 212 +++++++++++++++ scratch_test.cpp | 10 + seguidor_ruta.py | 74 ++++++ sim_trama.cpp | 47 ++++ teleop.py | 90 +++++++ test_motor.py | 12 + test_uart.py | 26 ++ test_uart2.py | 14 + test_uart_direct.py | 40 +++ test_uart_raw.py | 13 + 19 files changed, 1857 insertions(+) create mode 100644 AutoCalibracion/AutoCalibracion.ino create mode 100755 a.out create mode 100644 main.py create mode 100644 modulo_camara.py create mode 100644 modulo_esp32.py create mode 100644 modulo_imu.py create mode 100644 modulo_slam.py create mode 100644 out2.txt create mode 100644 output_test.txt create mode 100644 planificador_astar.py create mode 100644 scratch_test.cpp create mode 100644 seguidor_ruta.py create mode 100644 sim_trama.cpp create mode 100644 teleop.py create mode 100644 test_motor.py create mode 100644 test_uart.py create mode 100644 test_uart2.py create mode 100644 test_uart_direct.py create mode 100644 test_uart_raw.py diff --git a/AutoCalibracion/AutoCalibracion.ino b/AutoCalibracion/AutoCalibracion.ino new file mode 100644 index 0000000..44b9e3e --- /dev/null +++ b/AutoCalibracion/AutoCalibracion.ino @@ -0,0 +1,395 @@ +#include "freertos/FreeRTOS.h" +#include "freertos/semphr.h" +#include "freertos/task.h" +#include +#include + +// ========================================== +// PROTOCOLO UART +// ========================================== +// +// RX (RPi → ESP32): +// M:s1,f1,s2,f2,s3,f3,s4,f4\n +// s = signo → -1 | 0 | 1 +// f = fracción 0.000–1.000 de VEL_MAX_TICKS del motor +// S:\n → Stop inmediato +// C:\n → Iniciar autocalibración +// +// TX (ESP32 → RPi): +// T:t1,t2,t3,t4\n → Odometría acumulada (ticks con signo) +// K:v1,v2,v3,v4\n → VEL_MAX_TICKS de cada motor (ticks/s) — tras calibrar +// D:...\n → Debug (solo por USB/Serial) +// +// ========================================== + +// ─── FLAGS ──────────────────────────────────────────────────────────────────── +volatile bool iniciar_calibracion = false; +volatile bool en_calibracion = false; + +// ─── PINES ──────────────────────────────────────────────────────────────────── +const int PIN_RX_RPI = 44, PIN_TX_RPI = 43; +const int PIN_STBY_1 = 40, PIN_STBY_2 = 7; + +const int PIN_M1_IN1 = 15, PIN_M1_IN2 = 16, PIN_M1_PWM = 17, PIN_ENC1 = 9; +const int PIN_M2_IN1 = 6, PIN_M2_IN2 = 5, PIN_M2_PWM = 4, PIN_ENC2 = 10; +const int PIN_M3_IN1 = 41, PIN_M3_IN2 = 42, PIN_M3_PWM = 2, PIN_ENC3 = 36; +const int PIN_M4_IN1 = 39, PIN_M4_IN2 = 38, PIN_M4_PWM = 37, PIN_ENC4 = 35; + +const int FREC_PWM = 1000, RES_PWM = 8; + +const int IN1[] = {PIN_M1_IN1, PIN_M2_IN1, PIN_M3_IN1, PIN_M4_IN1}; +const int IN2[] = {PIN_M1_IN2, PIN_M2_IN2, PIN_M3_IN2, PIN_M4_IN2}; +const int PWM[] = {PIN_M1_PWM, PIN_M2_PWM, PIN_M3_PWM, PIN_M4_PWM}; +const int ENC[] = {PIN_ENC1, PIN_ENC2, PIN_ENC3, PIN_ENC4}; + +// ─── ENCODERS ───────────────────────────────────────────────────────────────── +volatile unsigned long pulsos[4] = {0, 0, 0, 0}; +void IRAM_ATTR isr_E1() { pulsos[0]++; } +void IRAM_ATTR isr_E2() { pulsos[1]++; } +void IRAM_ATTR isr_E3() { pulsos[2]++; } +void IRAM_ATTR isr_E4() { pulsos[3]++; } + +long global_ticks[4] = {0, 0, 0, 0}; +SemaphoreHandle_t mutexDatos; + +// ─── VELOCIDAD MÁXIMA CALIBRADA ─────────────────────────────────────────────── +// En ticks/s. La RPi la pide con C: y la recibe en K: +float VEL_MAX_TICKS[4] = {200.0f, 200.0f, 200.0f, 200.0f}; + +// Banda muerta por motor = 3% de VEL_MAX (se recalcula tras calibrar) +float BANDA_MUERTA[4] = {6.0f, 6.0f, 6.0f, 6.0f}; + +// ========================================== +// CLASE PID +// ========================================== +class PID { +public: + float kp, ki, kff; + float error_acumulado = 0.0f; + float setpoint = 0.0f; // ticks/s, con signo + + PID(float _kp, float _ki, float _kff) : kp(_kp), ki(_ki), kff(_kff) {} + + // vel_actual: ticks/s con signo + // banda : umbral bajo el cual el error se ignora (ticks/s) + int calcularPWM(float vel_actual, float dt, float banda) { + if (fabsf(setpoint) < 1.0f) { + error_acumulado = 0.0f; + return 0; + } + + float error = setpoint - vel_actual; + + // Banda muerta solo en régimen estacionario (motor ya girando) + if (fabsf(error) < banda && fabsf(vel_actual) > 5.0f) + error = 0.0f; + + error_acumulado += error * dt; + float lim = 200.0f / (ki + 0.0001f); + error_acumulado = constrain(error_acumulado, -lim, lim); + + float salida = kp * error + + ki * error_acumulado + + copysignf(fabsf(setpoint) * kff, setpoint); + + return (int)constrain(salida, -255.0f, 255.0f); + } + + void reset() { error_acumulado = 0.0f; setpoint = 0.0f; } +}; + +PID pid[4] = { + PID(2.0f, 1.5f, 0.0f), // M1 FL + PID(2.0f, 1.5f, 0.0f), // M2 FR + PID(2.0f, 1.5f, 0.0f), // M3 RL + PID(2.0f, 1.5f, 0.0f), // M4 RR +}; + +// ─── MOTOR ──────────────────────────────────────────────────────────────────── +void moverMotor(int idx, int val) { + if (abs(val) < 15) { + digitalWrite(IN1[idx], LOW); + digitalWrite(IN2[idx], LOW); + ledcWrite(PWM[idx], 0); + return; + } + if (val > 0) { digitalWrite(IN1[idx], HIGH); digitalWrite(IN2[idx], LOW); } + else { digitalWrite(IN1[idx], LOW); digitalWrite(IN2[idx], HIGH); val = -val; } + ledcWrite(PWM[idx], val); +} + +void pararTodos() { + for (int i = 0; i < 4; i++) moverMotor(i, 0); +} + +// ─── LECTURA ATÓMICA DE ENCODERS ────────────────────────────────────────────── +// portENTER_CRITICAL_ISR bloquea ambos cores en ESP32 (a diferencia de noInterrupts) +static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED; + +inline void leerPulsos(unsigned long dest[4]) { + portENTER_CRITICAL(&mux); + for (int i = 0; i < 4; i++) dest[i] = pulsos[i]; + portEXIT_CRITICAL(&mux); +} + +// ========================================== +// TAREA: CONTROL DE MOTORES (CORE 1) +// ========================================== +void tareaControlMotores(void *pvParameters) { + unsigned long p_ant[4] = {0, 0, 0, 0}; + static float v_f[4] = {0, 0, 0, 0}; + const float alpha = 0.35f; + const float dt = 0.05f; + const TickType_t xFrec = pdMS_TO_TICKS(50); + TickType_t xUltimo = xTaskGetTickCount(); + + for (;;) { + + // ──────────────────────────────────────────────────────────────────────── + // AUTOCALIBRACIÓN + // ──────────────────────────────────────────────────────────────────────── + if (iniciar_calibracion) { + en_calibracion = true; + for (int i = 0; i < 4; i++) pid[i].reset(); + pararTodos(); + vTaskDelay(pdMS_TO_TICKS(500)); + + unsigned long p_base[4]; + leerPulsos(p_base); + for (int i = 0; i < 4; i++) p_ant[i] = p_base[i]; + + // — Paso 1: fricción estática ———————————————————————————————————————— + int friccion[4] = {0, 0, 0, 0}; + for (int p = 20; p <= 130; p += 2) { + for (int i = 0; i < 4; i++) + if (friccion[i] == 0) { + digitalWrite(IN1[i], LOW); + digitalWrite(IN2[i], HIGH); + ledcWrite(PWM[i], p); + } + vTaskDelay(pdMS_TO_TICKS(60)); + + unsigned long p_act[4]; + leerPulsos(p_act); + bool todos = true; + for (int i = 0; i < 4; i++) { + if (friccion[i] == 0) { + if ((p_act[i] - p_ant[i]) > 1) friccion[i] = p; + else todos = false; + } + p_ant[i] = p_act[i]; + } + if (todos) break; + } + + // — Paso 2: velocidad máxima (kff y VEL_MAX_TICKS) —————————————————— + const int TEST_PWM = 200; + for (int i = 0; i < 4; i++) { + digitalWrite(IN1[i], LOW); + digitalWrite(IN2[i], HIGH); + ledcWrite(PWM[i], TEST_PWM); + } + vTaskDelay(pdMS_TO_TICKS(800)); // estabilización + + unsigned long p_ss1[4]; leerPulsos(p_ss1); + vTaskDelay(pdMS_TO_TICKS(300)); + unsigned long p_ss2[4]; leerPulsos(p_ss2); + + for (int i = 0; i < 4; i++) { + float vel_medida = (float)(p_ss2[i] - p_ss1[i]) / 0.3f; // ticks/s + + // VEL_MAX extrapolada a PWM 255 + VEL_MAX_TICKS[i] = (vel_medida > 10.0f) + ? vel_medida * (255.0f / (float)(TEST_PWM - friccion[i])) + : 200.0f; + VEL_MAX_TICKS[i] = constrain(VEL_MAX_TICKS[i], 50.0f, 1000.0f); + + // kff: PWM por ticks/s + pid[i].kff = (vel_medida > 10.0f) + ? (float)(TEST_PWM - friccion[i]) / vel_medida + : 2.0f; + pid[i].kff = constrain(pid[i].kff, 0.1f, 8.0f); + + // Banda muerta = 3% de VEL_MAX + BANDA_MUERTA[i] = VEL_MAX_TICKS[i] * 0.03f; + + pid[i].reset(); + } + + pararTodos(); + + // Reporte por USB + Serial.println("=== CALIBRACION COMPLETA ==="); + for (int i = 0; i < 4; i++) + Serial.printf("M%d fric=%d vel_max=%.1f kff=%.3f banda=%.1f\n", + i+1, friccion[i], VEL_MAX_TICKS[i], pid[i].kff, BANDA_MUERTA[i]); + + // ── Trama K: → RPi para que actualice su VEL_MAX ────────────────────── + // Tanto por USB como por Serial1 (UART RPi) + char buf[80]; + snprintf(buf, sizeof(buf), "K:%.1f,%.1f,%.1f,%.1f\n", + VEL_MAX_TICKS[0], VEL_MAX_TICKS[1], + VEL_MAX_TICKS[2], VEL_MAX_TICKS[3]); + Serial.print(buf); + Serial1.print(buf); + + leerPulsos(p_ant); + iniciar_calibracion = false; + en_calibracion = false; + } + // ──────────────────────────────────────────────────────────────────────── + + // Leer encoders + unsigned long p_act[4]; + leerPulsos(p_act); + + unsigned long d_p[4]; + for (int i = 0; i < 4; i++) { + d_p[i] = p_act[i] - p_ant[i]; + p_ant[i] = p_act[i]; + } + + // Dirección del setpoint para acumular odometría con signo + int d_f[4]; + for (int i = 0; i < 4; i++) + d_f[i] = (pid[i].setpoint > 1.0f) ? 1 : + (pid[i].setpoint < -1.0f) ? -1 : 0; + + if (xSemaphoreTake(mutexDatos, pdMS_TO_TICKS(5)) == pdTRUE) { + for (int i = 0; i < 4; i++) + global_ticks[i] += (long)d_p[i] * d_f[i]; + xSemaphoreGive(mutexDatos); + } + + // Velocidad filtrada con EMA + for (int i = 0; i < 4; i++) { + float v_cruda = ((float)d_p[i] * (float)d_f[i]) / dt; + v_f[i] = alpha * v_cruda + (1.0f - alpha) * v_f[i]; + } + + // PID y actuación + for (int i = 0; i < 4; i++) + moverMotor(i, pid[i].calcularPWM(v_f[i], dt, BANDA_MUERTA[i])); + + // Debug cada 500ms (10 ciclos × 50ms) + static int dbg = 0; + if (++dbg >= 10) { + dbg = 0; + Serial.printf("D:SP=%.0f,%.0f,%.0f,%.0f V=%.0f,%.0f,%.0f,%.0f\n", + pid[0].setpoint, pid[1].setpoint, pid[2].setpoint, pid[3].setpoint, + v_f[0], v_f[1], v_f[2], v_f[3]); + } + + vTaskDelayUntil(&xUltimo, xFrec); + } +} + +// ========================================== +// PARSEO DE TRAMA (Core 0) +// ========================================== +void procesarTrama(const String &t) { + if (t.length() < 2) return; + char cmd = t.charAt(0); + + if (cmd == 'C') { iniciar_calibracion = true; return; } + if (cmd == 'S') { for (int i = 0; i < 4; i++) pid[i].reset(); return; } + + if (cmd == 'M' && t.length() > 2) { + // Formato: M:s1,f1,s2,f2,s3,f3,s4,f4 + // s = -1, 0, o 1 | f = 0.000–1.000 + int s[4]; float f[4]; + if (sscanf(t.c_str() + 2, + "%d,%f,%d,%f,%d,%f,%d,%f", + &s[0], &f[0], &s[1], &f[1], + &s[2], &f[2], &s[3], &f[3]) != 8) + return; + + for (int i = 0; i < 4; i++) { + f[i] = constrain(f[i], 0.0f, 1.0f); + // Setpoint en ticks/s con signo aplicado directamente + pid[i].setpoint = (float)s[i] * f[i] * VEL_MAX_TICKS[i]; + } + return; + } +} + +// ========================================== +// TAREA: COMUNICACIÓN (Core 0) +// ========================================== +void tareaComunicacion(void *pvParameters) { + String tUSB = "", tRPI = ""; + unsigned long uEnvio = 0; + unsigned long ultimaVezRecibido = millis(); + + for (;;) { + // Telemetría cada 40ms + if (millis() - uEnvio >= 40) { + uEnvio = millis(); + long tk[4]; + if (xSemaphoreTake(mutexDatos, pdMS_TO_TICKS(5)) == pdTRUE) { + for (int i = 0; i < 4; i++) tk[i] = global_ticks[i]; + xSemaphoreGive(mutexDatos); + } + char buf[64]; + snprintf(buf, sizeof(buf), "T:%ld,%ld,%ld,%ld\n", tk[0], tk[1], tk[2], tk[3]); + Serial.print(buf); + Serial1.print(buf); + } + + // Lectura USB + while (Serial.available()) { + char c = Serial.read(); + if (c == '\n') { + tUSB.trim(); + if (tUSB.length()) { procesarTrama(tUSB); ultimaVezRecibido = millis(); } + tUSB = ""; + } else { tUSB += c; } + } + + // Lectura RPi + while (Serial1.available()) { + char c = Serial1.read(); + if (c == '\n') { + tRPI.trim(); + if (tRPI.length()) { procesarTrama(tRPI); ultimaVezRecibido = millis(); } + tRPI = ""; + } else { tRPI += c; } + } + + // Freno de emergencia (ignora calibración en curso) + // 800ms da margen para lags normales del SO de la RPi + if (!en_calibracion && millis() - ultimaVezRecibido > 800) + for (int i = 0; i < 4; i++) pid[i].reset(); + + vTaskDelay(pdMS_TO_TICKS(2)); + } +} + +// ========================================== +// SETUP +// ========================================== +void setup() { + Serial.begin(115200); + Serial1.begin(921600, SERIAL_8N1, PIN_RX_RPI, PIN_TX_RPI); + + pinMode(PIN_STBY_1, OUTPUT); digitalWrite(PIN_STBY_1, HIGH); + pinMode(PIN_STBY_2, OUTPUT); digitalWrite(PIN_STBY_2, HIGH); + + for (int i = 0; i < 4; i++) { + pinMode(IN1[i], OUTPUT); + pinMode(IN2[i], OUTPUT); + ledcAttach(PWM[i], FREC_PWM, RES_PWM); + pinMode(ENC[i], INPUT); + } + + attachInterrupt(digitalPinToInterrupt(ENC[0]), isr_E1, RISING); + attachInterrupt(digitalPinToInterrupt(ENC[1]), isr_E2, RISING); + attachInterrupt(digitalPinToInterrupt(ENC[2]), isr_E3, RISING); + attachInterrupt(digitalPinToInterrupt(ENC[3]), isr_E4, RISING); + + mutexDatos = xSemaphoreCreateMutex(); + xTaskCreatePinnedToCore(tareaControlMotores, "Ctrl", 4096, NULL, 2, NULL, 1); + xTaskCreatePinnedToCore(tareaComunicacion, "Com", 4096, NULL, 1, NULL, 0); +} + +void loop() { vTaskDelay(pdMS_TO_TICKS(10)); } \ No newline at end of file diff --git a/a.out b/a.out new file mode 100755 index 0000000000000000000000000000000000000000..3853bcb77e13678e484834d57418e7d26d20c61a GIT binary patch literal 76064 zcmeHN4Rl+@l^*@%k3fD%Ae2xdlC=BMJfn%GL&2GD=VNGdNl4!p> zZ$=(HNtBpw&z?Q=eB}G?ojZ5#d~@%ec~816vSF>)<6-LZv2QWr4lZycVc9Tx$)rx0 zt!A@Wn7x}_#HOR&kB`!G(-X3rqBEgOmk%H6x7tfwIbH0CnWAcRqSItsCDQm_N2(|j zvvK~Due(kY*`ees#G#P zFnE<^Wn;Z7>w~#WFyzd8A?#Dv)=f9D`pqNv{PE;N18u*#;OW3~&-`-B3hVdeH~B#J zNQW-cpCRrTBiLvusa?!V3>*OG$gjEkeRl}vE&ln~Y_?~9xq^=^f>17uY)&Jn&MjAE zAE+XK1l=p?e--)_hGfg(e)*e+H_|_j6V3 z+za0;*ds_3t+;^*a6?DRoiX&vvY;{SD3#@$}U{?01; zU#VjM{wn&ELjio;S8EkJ@ha;A_$%63QAPd&Y))m;iB794m2lcBw|sb0)LzATY;Z6X3e|R5xnv9` zv&nQ%TP$jaLa|;e%k8X{v~z8-NYui=NT`lSc`VbvgHvlR(jIGHAB(Pn4^y$WiotJeHjStX2;>yZ+2qU}bKg z3xh=ZXtuCj&$->3&1~n%^foJ%jB_j7Gtiev+i;NcY&iG9Au% z5}_KHi6?XYnOs7+f<%gC25bRkZH)>yNO-23XRUNkV#!WqZ^PAG!5!bvUkwd~Dd{IwhTT$4y@@^}c zV#qQ((@g|`q8ZUnVM#T&&q|UcHfV9A27-Mj^@pG745#VX$xNDAU70LU)6Dh^pa?M7 z$=UJPl~>XcZzZukF&K6Zd^CuZS3D55HaGLSU=3^C(ALzUK!YV_lYz>AL$f79?cUEWcfBXwFFeg<@B;#@fk ztseZ*e>!J+*w3(Ms{OS6y*p+z=$wR%E^2>OYrlW^J{(K^%r6e7)b3}G%l3BY|NLuP z!OdiI(M~p-PS6fMgIy?j`|`W-s4$IPF8NzTyN_K{wC80tej=tVQZO<9mza ztLGRW&Kq<+O9yaVcgg1!B_ERVg9T?}R`SDAJ`?EBrR0i#RO)+RU%Dvtp3>y%>`o2O zYI5~VMDoL$Tt7dIYVvoHqP&i2@_-V89@pf{H2DclzFd>flk=iSU3oMStAHMtBaAz!D-&r`-h+co+5ntZb+ zr>8Y_ZPDZxC?ROCCcjXV_iJ+XE`gc{HTgxF{*WfWSd;J4W5ahdcJHZMzqj|GkH4r?j-%~3(rtcsYkuC+>xKi~ zB`;DR^$~41qpitl6MPzcm&Rwo->&i7z(1_R{(H2!DcyEOiJ@V9IHi{Kx2 zc(u;eD6R9m+0gSB?NW=#2NK->mvt#vh?I_ zKW{wy=IB38GPD)(kopCw zTTi-|I=WwnOw8}#<0mlBOxG$fZo_nl$jt@eJRTcGevDDBfX8}&rf?f;7`u%PKaU)ix!s}X zc;o2;?H`nPlQpG6+<#KeN5nc}(4G4~Ks(k!#JY#E1K$IW8gf97VST=Y4}(6{8SLww z-vBRs01hOF?q63sKk|T?1cH z|A6Cb@Ue3Yep3zvaCQ=NLL1g2e{2nOG3PIkAB};Fq3hH|*L+Iy82a}>zoaHY-<`*& zPda(L89u#<`(vxZrzYwsk0bjEPpVj%tcw)us{XA;TvCiUaiVJBw?z$|hE?j4g0R%r_AJh}j0j%GS|E zU$~Igxfq`mx6r5gxn;@*?O(J;x{jVJgct0KH6FtJ6n_-+b8vI^VqlG2n7;4SCp^Qh z9B5GMEo$Y}G6xW|INOPN5Y7Tas`s=P6V||K3>Z-^1Q1U&mRh$Z*R$82rda(F8>VxP zSPS_1EBK`D_YnSSozQj!ygEmWVhf2DM7p!nqo-i-#jvzfj?dLWewBy)ju z#tw+@E||xIIAET|zUz*HF2-l@zY7KWxxiDP^U!z#l)nF6Jy9sE2HgdUn!)Y@J&5VV z-zXFY!KXm)1bqbb5zu3x$3VaH2iSq$>!3BD3r`dZ?VxAp(GT<(=pN9Se}q2hUeK9% znROWSGSG!MB(4Kp0lEeB&`FE~%1&V%&<8*dfj$HJThLKZKNj78x=^?XbTw!glrEgV z#O2x9!Tf_4dlsL)M940H)PAi{sHfJ(85ffteRsGYvKo@%B3T%pXCS*Cl&>$S3-#@U4rwVwd> z&llrtQRD284a+LxTW9!j6zdM#Q_#w z3I)$u?fyGF>^mO6;O1-rwf(^3h?2BGve}O+a!^Xv)I;)=r{<1n?4-y4eLwrM*N^*0 zy#7Oec1YYG@%oSY*^rOqyL|rF{cNu6iumNsqo*DJq#3qH-bk3(45_zBb)4(mDEU)z z;%Z*2C6KeEz8e2x$t%4#3(owYE<~p#KU3N(NZv2|9hdy068OBUU8?=BlY?@-|G(1R zG9DF>hgBdb!4&CCrXtKS(AwO5ZQ!z-x(3qrKp<2XtP9p$wQ@kPwI8dk57yKNLzl~j zKQ*L}dD!Yw^P-CnV|G@Q``BPbxxbii`j|d8t(a$}^6ABVES1k-Do#t~GntC-Qu!>V z@~Bjftd{v!Dxb|%-6@ry%~V{L%Fkg37w8h_-pA%JRj>3`9|qkY&?QQL9$R-wCHZ_- zv$T@@T=vL&D#;fxRTuSEA6v*&JuH=<$J}vA?LKxsQ}wG{;()eC> z4pVZK#*Th@{5bORc)02<$DPgg%X$bkaVa}{15zMKJ?LBcKTkO*_1A!s|89Asl!smV z4sMIEnUt5s96RN4xD9Qd3i0_McHc& zv#yH#XchUwCDMS)&Sh2P8>+~MT=ExT&x1POBehyQ4LGee|(~wuf z`43g(VLX5l4l3`KJ7(Ac$S;9ED*n~;%?nar)eTiH*WvlB(zp**k^hL~-tv9i=qq&xMdz8uwG? zwNLGl^ZEwlmHeEB2Ya%k_RSMg|6<51>EB9nZ~1xeH zksYa{lNkbkUEJW>^l=Cgtw9`Y_}HsiTu*75OKm9d|v? zLte>$7L8@?P%y)m#~J5VSCZRS4_iLSAWlLYvr;L(Jz;I--D#4gNtU&9_CR-cFvjAE zY@#QbvlCfv_wiUNlTPGdC!XOwsZ5uZ;&D5Z&2ei01~YyAsf3+~2Wu*8@a|+f$*pYG z+QAcPJG+B*XRW>jj}P?q?LZfe})EL4!3S-Yij1T!O$wk+irxBcrwigatR8XoNdLn z0#^hH;4V=me^+rDITQ-jc3HV(4C>@!TMRP^(PGfTCG8w$5w-9y5~}4Jc`TEL-ayQ5 zgENuHwoV>h&7*d0O?M`{-O9#!5 zo9GTAu?Give@+gr=H^OC=D5F`_b&;M{+HULG z&{d;q;Tb1-9TGj4usI4$kKG&TD@PZDqIfxrn#%N`dLXAV*|PPdy#w>sDxJHw)Fy6; zbfB))IaM^1%0;^M@et9eWEWKgD{FJgAEy;HudZG|`j=hjT4$X(@5<#=0!}4rLj*Ey z6xfWh9aIY8P3z^hf%T0i25;<+!~~dUKH*guON`Q9l5b9?AVKS}6X%M-!xPBT0SjjW zv3H&Ma3~s$v??N+5Fm1`^Ct}2z#SSJc7_D)y9L4mb1GT!7a zI;AaZWp1DgwjzBMa%EGLg1UWeM>G@VkibPmpM zk-jTsjR3He=Tq42i?Lauh!-=U!NC{jlSp!kIe>&i#mw~Ya88;)4L(O#mBU7dSX7

RF5+-x~nOve$w&P3?%M951D$*8DM-@>g>||!L^wDu|N@&-k zBF7`KieY-jC&K1X8{#fPn1~1@pVUcLM2;wuPUWU!HI*QzkTWWsq>k*ZbZl1nU5jS~ zXO{sqJk!mywEtl|7fe))#W=8udKOA%avW!<1nnJJE4xEHnUn@tT`jY^GFb_@O#TYn zcDQ--=EkNr9tze8%vDXcGiy^B%XZH9XC4mCwdL4NTB*m=KsWZqO;P@KkLQv0KHWb( zWAJFuiP$SX6e6_gk+9S1q7&my>FtOK-jT@ROcusB+&FLlISwoOVH;IyihD@|vL=R8 zu$_o>RIaXNk1MX4Bo~YdB^4>&)0auh;{<||kWmlIrMTdx@? z%C`xIUZ9G*{%nBRMiw4Jry2nsfUowU+8CZ9$qBgJ~UUSd`hyJeS zo!rxFF8T^WejQN!9f$fohwE!Pp{tL#3=Ccw7yGNwRlg@wS&>K^}_uKwzG zM2db?#=kP-9{-@LzxutAqHh0P%fX&Uf4s^Txdl@EUDB?T#7)MF!%gzakC)LvA*b9_ zzhAoRZTA09^i{b{W2o<^cgy|(xiBg|)wn8-CeVlGq{dgjhkEohSz~OSYr?9(qV)Zd z-hVO@oR04JSN()>{G(w=rp8y_$6O$+PU5BXUG6X+1SC`6Un)J-|0*SrRQ1=Xid|iG zog9C)1d8^Nzs5)TujYlX>x#bX{fFi`BWzSa%SJV>(xr4)b5}B=KMgpVXS+JMSoS}G CFBb3s literal 0 HcmV?d00001 diff --git a/main.py b/main.py new file mode 100644 index 0000000..7098e48 --- /dev/null +++ b/main.py @@ -0,0 +1,208 @@ +import time +import threading +import numpy as np +import cv2 +import math +from modulo_esp32 import ChasisESP32 +from modulo_camara import CamaraD415 +from modulo_slam import EvasorObstaculos + +# ── Constantes de velocidad ──────────────────────────────────────────────────── +VEL_NORMAL = 0.50 # m/s en camino libre +VEL_LENTA = 0.25 # m/s en precaución +VEL_GIRO = 1.50 # rad/s al girar + +# ── Mapa 2D local ────────────────────────────────────────────────────────────── +MAPA_W, MAPA_H = 500, 500 +ESCALA = 200 # píxeles por metro +ORIGEN = (MAPA_W // 2, MAPA_H // 2) + +# ── Objetos globales ─────────────────────────────────────────────────────────── +robot = ChasisESP32(puerto='/dev/ttyAMA0') +camara = CamaraD415() +evasor = EvasorObstaculos() +ejecutando = True + +# Flag de calibración en curso (evita que el autopiloto interfiera) +calibrando = False + +# ── Mapa acumulado ───────────────────────────────────────────────────────────── +mapa_fondo = np.zeros((MAPA_H, MAPA_W, 3), dtype=np.uint8) + + +# ── Hilo de hardware ────────────────────────────────────────────────────────── +def hilo_hardware(): + """ + Lee odometría y tramas de control continuamente. + Incluye las tramas K: (VEL_MAX) que procesa modulo_esp32 internamente. + """ + global ejecutando + while ejecutando: + robot.leer_odometria() + time.sleep(0.005) # 5ms → ~200 lecturas/s, suficiente para 40ms de telemetría + + +# ── Traducción CMD → (v, w) ──────────────────────────────────────────────────── +def cmd_a_velocidad(cmd: str) -> tuple[float, float]: + tabla = { + EvasorObstaculos.CMD_ADELANTE : (-VEL_NORMAL, 0.0), + EvasorObstaculos.CMD_ADELANTE_LENTO: (-VEL_LENTA, 0.0), + EvasorObstaculos.CMD_GIRO_IZQ : (0.0, -VEL_GIRO), + EvasorObstaculos.CMD_GIRO_DER : (0.0, VEL_GIRO), + EvasorObstaculos.CMD_RETROCEDER : (VEL_NORMAL, 0.0), + EvasorObstaculos.CMD_DETENIDO : (0.0, 0.0), + } + return tabla.get(cmd, (0.0, 0.0)) + + +# ── Dibujar mapa 2D ─────────────────────────────────────────────────────────── +def dibujar_mapa(x: float, y: float, theta: float) -> np.ndarray: + global mapa_fondo + + px = int(ORIGEN[0] + x * ESCALA) + py = int(ORIGEN[1] - y * ESCALA) + px_c = max(0, min(MAPA_W - 1, px)) + py_c = max(0, min(MAPA_H - 1, py)) + + cv2.circle(mapa_fondo, (px_c, py_c), 2, (80, 80, 80), -1) + mapa = mapa_fondo.copy() + + # Cuadrícula (cada metro) + for i in range(0, MAPA_W, ESCALA): + cv2.line(mapa, (i, 0), (i, MAPA_H), (30, 30, 30), 1) + cv2.line(mapa, (0, i), (MAPA_W, i), (30, 30, 30), 1) + cv2.drawMarker(mapa, ORIGEN, (60, 60, 60), cv2.MARKER_CROSS, 12, 1) + + # Robot + radio = 10 + cv2.circle(mapa, (px_c, py_c), radio, (0, 200, 255), 2) + fx = int(px_c + math.cos(theta) * radio * 1.6) + fy = int(py_c - math.sin(theta) * radio * 1.6) + cv2.arrowedLine(mapa, (px_c, py_c), (fx, fy), (0, 200, 255), 2, tipLength=0.4) + + # Banner de calibración en curso + if calibrando: + cv2.rectangle(mapa, (0, 0), (MAPA_W, 28), (0, 60, 120), -1) + cv2.putText(mapa, "⚙ CALIBRANDO — espera la trama K:", + (8, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.52, (0, 220, 255), 1) + + # HUD del evasor + evasor.dibujar_estado(mapa) + + # VEL_MAX actual (para diagnóstico visual) + vmax_txt = (f"VelMax izq={robot.vel_max_izq:.0f} " + f"der={robot.vel_max_der:.0f} ticks/s") + cv2.putText(mapa, vmax_txt, + (8, MAPA_H - 24), cv2.FONT_HERSHEY_SIMPLEX, 0.38, + (80, 160, 80), 1, cv2.LINE_AA) + + # Coordenadas + cv2.putText(mapa, + f"x={x:.2f}m y={y:.2f}m θ={math.degrees(theta):.1f}°", + (8, MAPA_H - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.42, + (140, 140, 140), 1, cv2.LINE_AA) + + return mapa + + +# ── Calibración asíncrona ───────────────────────────────────────────────────── +def lanzar_calibracion(): + """ + Ejecuta la calibración en un hilo aparte para no bloquear el bucle principal. + El ESP32 tarda ~2s en calibrar y responde con la trama K: automáticamente. + modulo_esp32.leer_odometria() la captura en el hilo de hardware. + """ + global calibrando + calibrando = True + print("\n⚙️ Iniciando calibración (el ESP32 responderá con K: al terminar)...") + robot.calibrar_pid() + # Esperamos hasta 8s; si el ESP32 actualiza VEL_MAX antes, el flag + # se puede bajar manualmente, pero aquí simplemente esperamos el tiempo máximo. + time.sleep(8.0) + calibrando = False + print("\n✅ Tiempo de calibración agotado — VEL_MAX activa en el sistema.") + + +# ── Main ─────────────────────────────────────────────────────────────────────── +def main(): + global ejecutando + print("\n🚀 INICIANDO SISTEMA AUTÓNOMO...") + print("Teclas (ventana Mapa): [a] Auto | [m] Manual | [c] Calibrar | [q] Salir") + + hilo = threading.Thread(target=hilo_hardware, daemon=True) + hilo.start() + + modo_autonomo = False + hilo_calib = None + + try: + while True: + escaner, frame_video = camara.obtener_datos() + + # Vídeo de profundidad + if frame_video is not None: + frame_video[235:245, :, :] = frame_video[240, :, :] + cv2.imshow("Vision RealSense D415", frame_video) + + # Decisión del evasor + estado = evasor.decidir_movimiento(escaner) + cmd = estado["comando"] + + # Autopiloto — bloqueado durante calibración + if modo_autonomo and not calibrando: + v, w = cmd_a_velocidad(cmd) + robot.enviar_velocidad(v, w) + print(f"\r🤖 [{cmd}] — {estado['razon'][:55]:<55}", end="") + elif modo_autonomo and calibrando: + # Detener mientras calibra para no interferir + robot.enviar_velocidad(0.0, 0.0) + + # Mapa 2D + mapa = dibujar_mapa(robot.x, robot.y, robot.theta) + cv2.imshow("Mapa SLAM", mapa) + + # Teclado + tecla = cv2.waitKey(1) & 0xFF + + if tecla == ord('a'): + if calibrando: + print("\n⚠️ Calibración en curso — espera antes de activar autopiloto.") + else: + print("\n🤖 AUTOPILOTO ON") + modo_autonomo = True + + elif tecla == ord('m'): + print("\n🛑 MODO MANUAL") + modo_autonomo = False + robot.enviar_velocidad(0.0, 0.0) + + elif tecla == ord('c'): + if calibrando: + print("\n⚠️ Ya hay una calibración en curso.") + else: + # Detener robot primero + modo_autonomo = False + robot.enviar_velocidad(0.0, 0.0) + time.sleep(0.2) + # Lanzar calibración en hilo para no bloquear la UI + hilo_calib = threading.Thread(target=lanzar_calibracion, daemon=True) + hilo_calib.start() + + elif tecla == ord('q'): + break + + except KeyboardInterrupt: + pass + finally: + ejecutando = False + hilo.join(timeout=1.0) + if hilo_calib and hilo_calib.is_alive(): + hilo_calib.join(timeout=2.0) + robot.enviar_velocidad(0.0, 0.0) + camara.cerrar() + cv2.destroyAllWindows() + print("\n🔌 Apagado.") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/modulo_camara.py b/modulo_camara.py new file mode 100644 index 0000000..174cfc4 --- /dev/null +++ b/modulo_camara.py @@ -0,0 +1,29 @@ +import pyrealsense2 as rs +import numpy as np + +class CamaraD415: + def __init__(self): + self.pipeline = rs.pipeline() + config = rs.config() + config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15) + self.colorizer = rs.colorizer() + try: + self.pipeline.start(config) + print("✅ Cámara D415 Iniciada (Modo Video + Lidar)") + except Exception as e: print(f"❌ Error Cámara: {e}") + + def obtener_datos(self): + try: + frames = self.pipeline.wait_for_frames(timeout_ms=1000) + depth_frame = frames.get_depth_frame() + if not depth_frame: return None, None + + depth_color_frame = self.colorizer.colorize(depth_frame) + imagen_video = np.asanyarray(depth_color_frame.get_data()) + + profundidad_matriz = np.asanyarray(depth_frame.get_data()) + fila_central = profundidad_matriz[240, :] * 0.001 + return fila_central, imagen_video + except: return None, None + + def cerrar(self): self.pipeline.stop() \ No newline at end of file diff --git a/modulo_esp32.py b/modulo_esp32.py new file mode 100644 index 0000000..f0c6231 --- /dev/null +++ b/modulo_esp32.py @@ -0,0 +1,192 @@ +import serial +import math + + +class ChasisESP32: + """ + Interfaz UART con el ESP32. + + Protocolo de TX (RPi → ESP32): + M:s1,f1,s2,f2,s3,f3,s4,f4\\n + s = signo → -1 | 0 | 1 + f = fracción de VEL_MAX → 0.000 – 1.000 + S:\\n → Stop inmediato + C:\\n → Iniciar autocalibración + + Protocolo de RX (ESP32 → RPi): + T:t1,t2,t3,t4\\n → Odometría acumulada (ticks con signo) + K:v1,v2,v3,v4\\n → VEL_MAX_TICKS calibrada por motor (ticks/s) + D:...\\n → Mensajes de debug del ESP32 + """ + + # ── Geometría ────────────────────────────────────────────────────────────── + TICKS_POR_VUELTA = 20.0 + RADIO_RUEDA = 0.03 # metros + ANCHO_EJE = 0.11 # metros (distancia entre ruedas del mismo eje) + + # VEL_MAX_TICKS se actualiza cuando el ESP32 envía la trama K: + # Valor inicial conservador: 200 ticks/s + # M1=FL, M2=FR, M3=RL, M4=RR + _VEL_MAX_TICKS = [200.0, 200.0, 200.0, 200.0] + + def __init__(self, puerto: str = '/dev/ttyAMA0', baudrate: int = 921600): + self.METROS_POR_TICK = (2.0 * math.pi * self.RADIO_RUEDA) / self.TICKS_POR_VUELTA + + try: + self.puerto = serial.Serial(puerto, baudrate, timeout=0.05) + print(f"✅ Conexión UART activa en {puerto}") + except Exception as e: + print(f"❌ Error UART: {e}") + self.puerto = None + + self.x, self.y, self.theta = 0.0, 0.0, 0.0 + self.ticks_anteriores = [0, 0, 0, 0] + self.primera_lectura = True + + # ── Velocidad máxima (promedio por lado) ─────────────────────────────────── + @property + def vel_max_izq(self) -> float: + """Ticks/s máximos del lado izquierdo (promedio M1 y M3).""" + return (self._VEL_MAX_TICKS[0] + self._VEL_MAX_TICKS[2]) / 2.0 + + @property + def vel_max_der(self) -> float: + """Ticks/s máximos del lado derecho (promedio M2 y M4).""" + return (self._VEL_MAX_TICKS[1] + self._VEL_MAX_TICKS[3]) / 2.0 + + # ── Lectura de odometría y tramas de control ─────────────────────────────── + def leer_odometria(self) -> bool: + """ + Lee una línea del ESP32 y la procesa. + Retorna True si se actualizó la odometría. + """ + if not (self.puerto and self.puerto.in_waiting > 0): + return False + + try: + linea = self.puerto.readline().decode('utf-8', errors='ignore').strip() + except Exception: + return False + + # ── T: odometría ────────────────────────────────────────────────────── + if linea.startswith("T:"): + partes = linea[2:].split(',') + if len(partes) != 4: + return False + try: + ticks_actuales = [int(p) for p in partes] + except ValueError: + return False + + if self.primera_lectura: + self.ticks_anteriores = ticks_actuales + self.primera_lectura = False + return True + + delta = [ticks_actuales[i] - self.ticks_anteriores[i] for i in range(4)] + self.ticks_anteriores = ticks_actuales + + # Desplazamiento lineal de cada lado (metros) + # M1(FL) y M3(RL) → izquierda + # M2(FR) y M4(RR) → derecha + d_izq = ((delta[0] + delta[2]) / 2.0) * self.METROS_POR_TICK + d_der = ((delta[1] + delta[3]) / 2.0) * self.METROS_POR_TICK + + d_centro = (d_izq + d_der) / 2.0 + d_theta = (d_der - d_izq) / self.ANCHO_EJE + + self.x += d_centro * math.cos(self.theta + d_theta / 2.0) + self.y += d_centro * math.sin(self.theta + d_theta / 2.0) + self.theta += d_theta + return True + + # ── K: VEL_MAX calibrada por el ESP32 ───────────────────────────────── + elif linea.startswith("K:"): + partes = linea[2:].split(',') + if len(partes) == 4: + try: + nuevos = [float(p) for p in partes] + # Validar que los valores sean razonables (10–2000 ticks/s) + if all(10.0 < v < 2000.0 for v in nuevos): + self._VEL_MAX_TICKS = nuevos + print( + f"\n✅ VEL_MAX actualizada → " + f"M1={nuevos[0]:.1f} M2={nuevos[1]:.1f} " + f"M3={nuevos[2]:.1f} M4={nuevos[3]:.1f} ticks/s" + ) + else: + print(f"\n⚠️ VEL_MAX recibida fuera de rango: {nuevos} — ignorada") + except ValueError: + pass + + # ── D: debug del ESP32 ──────────────────────────────────────────────── + elif linea.startswith("D:"): + print(f"\r[ESP32] {linea} ", end="") + + return False + + # ── Envío de velocidad diferencial ───────────────────────────────────────── + def enviar_velocidad(self, v: float, w: float) -> None: + """ + Convierte velocidad lineal (m/s) y angular (rad/s) al protocolo M:. + + Cinemática diferencial: + v_izq = v - w * (ANCHO_EJE / 2) + v_der = v + w * (ANCHO_EJE / 2) + + Cada rueda se envía con: + s = signo (-1, 0, 1) + f = |vel_rueda_ticks| / VEL_MAX_TICKS del motor (0.0 – 1.0) + + Trama resultante: + M:s1,f1,s2,f2,s3,f3,s4,f4\\n + """ + if not self.puerto: + return + + # Velocidades en m/s para cada lado + v_izq_ms = v - w * (self.ANCHO_EJE / 2.0) + v_der_ms = v + w * (self.ANCHO_EJE / 2.0) + + # Convertir a ticks/s + v_izq_ticks = v_izq_ms / self.METROS_POR_TICK + v_der_ticks = v_der_ms / self.METROS_POR_TICK + + # Velocidades por motor (izq = M1, M3 / der = M2, M4) + vel_motores = [v_izq_ticks, v_der_ticks, v_izq_ticks, v_der_ticks] + + # Umbral mínimo de movimiento: 0.5 ticks/s ≈ movimiento nulo + UMBRAL = 0.5 + + partes = [] + todos_cero = True + for i, vel in enumerate(vel_motores): + if abs(vel) < UMBRAL: + s, f = 0, 0.0 + else: + s = 1 if vel > 0 else -1 + # Normalizar respecto a la VEL_MAX de cada motor específico + f = min(abs(vel) / self._VEL_MAX_TICKS[i], 1.0) + todos_cero = False + partes.append(f"{s},{f:.3f}") + + if todos_cero: + cmd = "S:\n" + else: + cmd = "M:" + ",".join(partes) + "\n" + + print(f"\r[UART TX] → {cmd.strip():<55}", end="", flush=True) + try: + self.puerto.write(cmd.encode('utf-8')) + except Exception as e: + print(f"\n❌ Error al escribir UART: {e}") + + # ── Calibración ─────────────────────────────────────────────────────────── + def calibrar_pid(self) -> None: + """Solicita autocalibración al ESP32. La respuesta K: actualizará VEL_MAX.""" + if self.puerto: + try: + self.puerto.write(b"C:\n") + print("\n⚙️ Solicitud de calibración enviada al ESP32.") + except Exception as e: + print(f"\n❌ Error al enviar calibración: {e}") \ No newline at end of file diff --git a/modulo_imu.py b/modulo_imu.py new file mode 100644 index 0000000..42f9300 --- /dev/null +++ b/modulo_imu.py @@ -0,0 +1,296 @@ +""" +modulo_imu.py — Sistema Vestibular del Robot +============================================= +Interfaz con el sensor MPU6050 via I2C en Raspberry Pi 5. + +Responsabilidades: + - Leer velocidad angular del eje X (Yaw) del giroscopio + - Integrar numéricamente para obtener ángulo absoluto + - Detectar y compensar el drift del giroscopio (bias) + - Exponer theta_rad (radianes) para fusión de sensores en SLAM + +Conexión hardware: + MPU6050 VCC → Pin 1 (3.3V) + MPU6050 GND → Pin 6 (GND) + MPU6050 SDA → Pin 3 (GPIO2, I2C1 SDA) + MPU6050 SCL → Pin 5 (GPIO3, I2C1 SCL) + MPU6050 AD0 → GND (dirección I2C = 0x68) + +Dependencias: + pip install smbus2 +""" + +import smbus2 +import time +import threading +import logging +import math + +logger = logging.getLogger(__name__) + +# ── Registros del MPU6050 ───────────────────────────────────────────────────── +MPU6050_ADDR = 0x68 # AD0 en GND → 0x68; AD0 en VCC → 0x69 +REG_PWR_MGMT_1 = 0x6B +REG_SMPLRT_DIV = 0x19 +REG_CONFIG = 0x1A +REG_GYRO_CONFIG = 0x1B +REG_ACCEL_CONFIG = 0x1C +REG_GYRO_XOUT_H = 0x43 +REG_GYRO_YOUT_H = 0x45 +REG_GYRO_ZOUT_H = 0x47 +REG_ACCEL_ZOUT_H = 0x3F +REG_WHO_AM_I = 0x75 + +# ── Escalas del giroscopio ──────────────────────────────────────────────────── +# FS_SEL: 0 = ±250°/s, 1 = ±500°/s, 2 = ±1000°/s, 3 = ±2000°/s +GYRO_FS_SEL = 0 # ±250°/s → mejor resolución para robot lento +GYRO_SCALE = {0: 131.0, 1: 65.5, 2: 32.8, 3: 16.4} # LSB/(°/s) + +ACCEL_FS_SEL = 0 # ±2g +ACCEL_SCALE = {0: 16384.0, 1: 8192.0, 2: 4096.0, 3: 2048.0} # LSB/g + +# ── Configuración del filtro DLPF ───────────────────────────────────────────── +# DLPF_CFG 3 → Ancho de banda 44 Hz, delay 4.9ms — buen balance ruido/respuesta +DLPF_CFG = 3 + +# ── Número de muestras para calibración de bias ─────────────────────────────── +CALIBRATION_SAMPLES = 500 + + +class IMU: + """ + Interfaz de alto nivel con el MPU6050. + + Uso básico: + imu = IMU(bus=1) + imu.iniciar() + ... + theta = imu.theta_rad # ángulo Yaw integrado en radianes + imu.detener() + """ + + def __init__(self, bus: int = 1, direccion: int = MPU6050_ADDR, + frecuencia_hz: float = 200.0): + """ + Args: + bus: Número del bus I2C (1 para RPi 5, pines 3/5). + direccion: Dirección I2C del MPU6050 (0x68 o 0x69). + frecuencia_hz: Frecuencia de muestreo del bucle de integración. + """ + self._bus_num = bus + self._addr = direccion + self._frecuencia = frecuencia_hz + self._dt = 1.0 / frecuencia_hz + + self._bus: smbus2.SMBus | None = None + + # Estado angular (acceso thread-safe) + self._lock = threading.Lock() + self._theta_rad = 0.0 # Yaw integrado + self._omega_x_rads = 0.0 # Velocidad angular actual (rad/s) + self._accel_z_ms2 = 0.0 # Aceleración lineal eje Z (m/s²) + + # Calibración (bias en reposo) + self._bias_x_deg_s = 0.0 + self._bias_accel_z_ms2 = 0.0 + + # Escala activa + self._escala = GYRO_SCALE[GYRO_FS_SEL] + self._escala_accel = ACCEL_SCALE[ACCEL_FS_SEL] + + # Hilo de lectura + self._hilo: threading.Thread | None = None + self._activo = False + + # ── Propiedades públicas (thread-safe) ──────────────────────────────────── + + @property + def theta_rad(self) -> float: + """Ángulo Yaw acumulado en radianes. Positivo = giro anti-horario.""" + with self._lock: + return self._theta_rad + + @property + def theta_deg(self) -> float: + """Ángulo Yaw acumulado en grados.""" + return math.degrees(self.theta_rad) + + @property + def omega_x_rads(self) -> float: + """Velocidad angular instantánea en el eje X (rad/s).""" + with self._lock: + return self._omega_x_rads + + @property + def accel_z_ms2(self) -> float: + """Aceleración lineal instantánea en el eje Z (m/s²).""" + with self._lock: + return self._accel_z_ms2 + + # ── Inicialización ──────────────────────────────────────────────────────── + + def iniciar(self) -> None: + """ + Abre el bus I2C, configura el MPU6050, calibra el bias + y lanza el hilo de integración en segundo plano. + """ + logger.info("IMU: Abriendo bus I2C-%d, dirección 0x%02X", self._bus_num, self._addr) + self._bus = smbus2.SMBus(self._bus_num) + self._verificar_chip() + self._configurar_chip() + logger.info("IMU: Calibrando bias de giroscopio y acelerómetro (%d muestras)...", CALIBRATION_SAMPLES) + self._calibrar_bias() + logger.info("IMU: Bias Eje-X = %.4f °/s | Bias Accel Z = %.4f m/s²", self._bias_x_deg_s, self._bias_accel_z_ms2) + + self._activo = True + self._hilo = threading.Thread(target=self._bucle_integracion, + name="hilo-imu", daemon=True) + self._hilo.start() + logger.info("IMU: Hilo de integración iniciado a %.0f Hz", self._frecuencia) + + def detener(self) -> None: + """Detiene el hilo de lectura y cierra el bus I2C.""" + self._activo = False + if self._hilo: + self._hilo.join(timeout=2.0) + if self._bus: + self._bus.close() + logger.info("IMU: Detenida.") + + def resetear_angulo(self, valor_rad: float = 0.0) -> None: + """ + Reestablece el ángulo Yaw integrado a un valor arbitrario. + Útil cuando el SLAM cierra un loop y corrige la pose. + """ + with self._lock: + self._theta_rad = valor_rad + + # ── Configuración del chip ──────────────────────────────────────────────── + + def _verificar_chip(self) -> None: + who = self._bus.read_byte_data(self._addr, REG_WHO_AM_I) + if who != 0x68: + raise RuntimeError( + f"IMU: WHO_AM_I=0x{who:02X} — ¿MPU6050 conectado y AD0 en GND?" + ) + logger.debug("IMU: WHO_AM_I OK (0x68)") + + def _configurar_chip(self) -> None: + # Despertar el chip (sale del modo sleep) + self._bus.write_byte_data(self._addr, REG_PWR_MGMT_1, 0x00) + time.sleep(0.1) + + # Seleccionar reloj del giroscopio eje X (más estable que el oscilador interno) + self._bus.write_byte_data(self._addr, REG_PWR_MGMT_1, 0x01) + + # Sample Rate Divider: SMPLRT_DIV=0 → Fsample = 8 kHz / (1+0) = 8 kHz + # (El hilo de Python muestrea a 200 Hz; el chip siempre está listo) + self._bus.write_byte_data(self._addr, REG_SMPLRT_DIV, 0x00) + + # Filtro pasa-bajas digital (DLPF) + self._bus.write_byte_data(self._addr, REG_CONFIG, DLPF_CFG) + + # Rango del giroscopio y acelerómetro + self._bus.write_byte_data(self._addr, REG_GYRO_CONFIG, GYRO_FS_SEL << 3) + self._bus.write_byte_data(self._addr, REG_ACCEL_CONFIG, ACCEL_FS_SEL << 3) + + logger.debug("IMU: Chip configurado. Rango=±%s°/s, DLPF=%d", + {0: 250, 1: 500, 2: 1000, 3: 2000}[GYRO_FS_SEL], DLPF_CFG) + + # ── Lectura de registros ────────────────────────────────────────────────── + + def _leer_gyro_x_raw(self) -> int: + """Lee los 2 bytes del giroscopio eje X y devuelve el entero con signo.""" + high = self._bus.read_byte_data(self._addr, REG_GYRO_XOUT_H) + low = self._bus.read_byte_data(self._addr, REG_GYRO_XOUT_H + 1) + valor = (high << 8) | low + # Convertir a complemento a dos + if valor >= 0x8000: + valor -= 0x10000 + return valor + + def _leer_gyro_x_deg_s(self) -> float: + """Devuelve la velocidad angular del eje X en grados/segundo.""" + return self._leer_gyro_x_raw() / self._escala + + def _leer_accel_z_raw(self) -> int: + """Lee los 2 bytes del acelerómetro eje Z y devuelve el entero con signo.""" + high = self._bus.read_byte_data(self._addr, REG_ACCEL_ZOUT_H) + low = self._bus.read_byte_data(self._addr, REG_ACCEL_ZOUT_H + 1) + valor = (high << 8) | low + if valor >= 0x8000: + valor -= 0x10000 + return valor + + def _leer_accel_z_ms2_crudo(self) -> float: + """Devuelve la aceleración cruda del eje Z en m/s².""" + g_force = self._leer_accel_z_raw() / self._escala_accel + return g_force * 9.80665 + + # ── Calibración ─────────────────────────────────────────────────────────── + + def _calibrar_bias(self) -> None: + """ + Estima el drift (offset) del giroscopio y la gravedad estática del acelerómetro en reposo. + El robot DEBE estar completamente inmóvil durante este proceso. + """ + acumulador_gyro = 0.0 + acumulador_accel = 0.0 + for _ in range(CALIBRATION_SAMPLES): + acumulador_gyro += self._leer_gyro_x_deg_s() + acumulador_accel += self._leer_accel_z_ms2_crudo() + time.sleep(0.002) # ~500 Hz durante calibración + self._bias_x_deg_s = acumulador_gyro / CALIBRATION_SAMPLES + self._bias_accel_z_ms2 = acumulador_accel / CALIBRATION_SAMPLES + + # ── Bucle de integración ────────────────────────────────────────────────── + + def _bucle_integracion(self) -> None: + """ + Hilo principal de integración. + Lee ω_x, resta el bias, integra numéricamente y actualiza theta_rad. + + Δθ = -(ω_x − bias) × Δt + θ_actual = θ_anterior + Δθ + """ + intervalo = self._dt + siguiente_tick = time.perf_counter() + intervalo + + while self._activo: + ahora = time.perf_counter() + + try: + # Giroscopio (sentido del ángulo invertido) + omega_x_deg_s = self._leer_gyro_x_deg_s() - self._bias_x_deg_s + omega_x_rads = -math.radians(omega_x_deg_s) + delta_theta = omega_x_rads * intervalo + + # Acelerómetro + accel_z = self._leer_accel_z_ms2_crudo() - self._bias_accel_z_ms2 + + with self._lock: + self._theta_rad += delta_theta + self._omega_x_rads = omega_x_rads + self._accel_z_ms2 = accel_z + + except Exception as e: + logger.warning("IMU: Error de lectura I2C: %s", e) + + # Espera precisa hasta el siguiente tick + siguiente_tick += intervalo + pausa = siguiente_tick - time.perf_counter() + if pausa > 0: + time.sleep(pausa) + + # ── Diagnóstico ─────────────────────────────────────────────────────────── + + def diagnostico(self) -> dict: + """Devuelve un snapshot del estado actual para logging/debug.""" + with self._lock: + return { + "theta_deg": math.degrees(self._theta_rad), + "theta_rad": self._theta_rad, + "omega_x_rads": self._omega_x_rads, + "bias_x_deg_s": self._bias_x_deg_s, + "activo": self._activo, + } diff --git a/modulo_slam.py b/modulo_slam.py new file mode 100644 index 0000000..4e5dfaf --- /dev/null +++ b/modulo_slam.py @@ -0,0 +1,196 @@ +import math +import numpy as np + + +class EvasorObstaculos: + """ + Módulo de evasión de obstáculos para robot con cámara RealSense D415. + Analiza el escaneo de profundidad y genera comandos de movimiento seguros. + """ + + # ── Zonas del campo visual (en fracción de los 640 píxeles) ────────────── + ZONA_IZQ = (0, 213) # píxeles 0–212 + ZONA_CENT = (213, 427) # píxeles 213–426 + ZONA_DER = (427, 640) # píxeles 427–639 + + # ── Umbrales de distancia (metros) ────────────────────────────────────── + DIST_PELIGRO = 0.45 # detener / maniobrar urgente + DIST_PRECAUCION = 0.80 # reducir velocidad / preparar giro + DIST_LIBRE = 1.20 # camino despejado + + # ── Comandos de salida ─────────────────────────────────────────────────── + CMD_ADELANTE = "ADELANTE" + CMD_ADELANTE_LENTO= "ADELANTE_LENTO" + CMD_GIRO_IZQ = "GIRAR_IZQUIERDA" + CMD_GIRO_DER = "GIRAR_DERECHA" + CMD_RETROCEDER = "RETROCEDER" + CMD_DETENIDO = "DETENIDO" + + def __init__(self, + dist_peligro: float = None, + dist_precaucion: float = None, + dist_libre: float = None): + self.dist_peligro = dist_peligro or self.DIST_PELIGRO + self.dist_precaucion = dist_precaucion or self.DIST_PRECAUCION + self.dist_libre = dist_libre or self.DIST_LIBRE + + # Último estado para logging / depuración + self.ultimo_estado: dict = {} + + # ──────────────────────────────────────────────────────────────────────── + # Método principal + # ──────────────────────────────────────────────────────────────────────── + def decidir_movimiento(self, escaner: np.ndarray) -> dict: + """ + Recibe el array de 640 distancias (metros) de la cámara y + devuelve un diccionario con el comando y métricas de la decisión. + + Returns + ------- + { + "comando" : str, # CMD_* constante + "dist_izq" : float, # distancia mínima zona izquierda + "dist_centro": float, # distancia mínima zona central + "dist_der" : float, # distancia mínima zona derecha + "obstaculo" : bool, # True si hay peligro inminente + "razon" : str, # explicación legible + } + """ + if escaner is None or len(escaner) != 640: + return self._estado(self.CMD_DETENIDO, 0, 0, 0, True, + "Escáner no disponible") + + # ── 1. Distancias mínimas por zona (ignorar lecturas inválidas) ───── + dist_izq = self._min_valida(escaner, *self.ZONA_IZQ) + dist_centro = self._min_valida(escaner, *self.ZONA_CENT) + dist_der = self._min_valida(escaner, *self.ZONA_DER) + + # ── 2. Árbol de decisión ───────────────────────────────────────────── + cmd, razon = self._arbol_decision(dist_izq, dist_centro, dist_der) + + hay_peligro = (dist_centro < self.dist_peligro or + dist_izq < self.dist_peligro or + dist_der < self.dist_peligro) + + self.ultimo_estado = self._estado( + cmd, dist_izq, dist_centro, dist_der, hay_peligro, razon) + return self.ultimo_estado + + # ──────────────────────────────────────────────────────────────────────── + # Árbol de decisión + # ──────────────────────────────────────────────────────────────────────── + def _arbol_decision(self, + izq: float, + centro: float, + der: float) -> tuple[str, str]: + """Lógica de prioridad para seleccionar comando.""" + + # ── Zona central bloqueada ─────────────────────────────────────────── + if centro < self.dist_peligro: + + # Ambos lados bloqueados → retroceder + if izq < self.dist_peligro and der < self.dist_peligro: + return self.CMD_RETROCEDER, \ + f"Todos los lados bloqueados ({centro:.2f}m)" + + # Izquierda libre → girar izquierda + if izq >= der: + return self.CMD_GIRO_IZQ, \ + f"Centro bloqueado ({centro:.2f}m), izq más libre ({izq:.2f}m)" + + # Derecha libre → girar derecha + return self.CMD_GIRO_DER, \ + f"Centro bloqueado ({centro:.2f}m), der más libre ({der:.2f}m)" + + # ── Precaución central ─────────────────────────────────────────────── + if centro < self.dist_precaucion: + + if izq > self.dist_libre and der <= self.dist_precaucion: + return self.CMD_GIRO_IZQ, \ + f"Precaución centro ({centro:.2f}m), girando izq" + + if der > self.dist_libre and izq <= self.dist_precaucion: + return self.CMD_GIRO_DER, \ + f"Precaución centro ({centro:.2f}m), girando der" + + return self.CMD_ADELANTE_LENTO, \ + f"Precaución ({centro:.2f}m), avanzando despacio" + + # ── Centro libre — revisar laterales ──────────────────────────────── + if izq < self.dist_peligro: + return self.CMD_GIRO_DER, \ + f"Obstáculo lateral izq ({izq:.2f}m)" + + if der < self.dist_peligro: + return self.CMD_GIRO_IZQ, \ + f"Obstáculo lateral der ({der:.2f}m)" + + # ── Camino despejado ───────────────────────────────────────────────── + return self.CMD_ADELANTE, \ + f"Camino libre (centro={centro:.2f}m)" + + # ──────────────────────────────────────────────────────────────────────── + # Utilidades + # ──────────────────────────────────────────────────────────────────────── + def _min_valida(self, escaner: np.ndarray, + inicio: int, fin: int) -> float: + """Distancia mínima en la zona [inicio, fin) ignorando valores nulos.""" + zona = escaner[inicio:fin] + validos = zona[(zona > 0.05) & (zona < 10.0)] + return float(np.min(validos)) if len(validos) else 10.0 + + @staticmethod + def _estado(cmd, izq, centro, der, peligro, razon) -> dict: + return { + "comando" : cmd, + "dist_izq" : round(float(izq), 3), + "dist_centro": round(float(centro), 3), + "dist_der" : round(float(der), 3), + "obstaculo" : peligro, + "razon" : razon, + } + + # ──────────────────────────────────────────────────────────────────────── + # Visualización en el mapa SLAM + # ──────────────────────────────────────────────────────────────────────── + def dibujar_estado(self, mapa: np.ndarray, + pos_x: int = 10, pos_y: int = 20) -> None: + """ + Superpone el estado de evasión sobre el mapa OpenCV existente. + Llama a esto después de AlgoritmoSLAM.dibujar_robot_y_entorno(). + """ + import cv2 + + e = self.ultimo_estado + if not e: + return + + COLOR_OK = (0, 220, 0) + COLOR_PRECAU = (0, 200, 255) + COLOR_PELIGRO = (0, 0, 255) + + color = COLOR_PELIGRO if e["obstaculo"] else ( + COLOR_PRECAU if e["dist_centro"] < self.dist_precaucion + else COLOR_OK) + + lineas = [ + f"CMD : {e['comando']}", + f"IZQ : {e['dist_izq']:.2f}m", + f"CENT: {e['dist_centro']:.2f}m", + f"DER : {e['dist_der']:.2f}m", + e["razon"], + ] + + # Fondo semitransparente + overlay = mapa.copy() + cv2.rectangle(overlay, + (pos_x - 4, pos_y - 16), + (pos_x + 250, pos_y + len(lineas) * 18 + 4), + (30, 30, 30), -1) + cv2.addWeighted(overlay, 0.6, mapa, 0.4, 0, mapa) + + for i, linea in enumerate(lineas): + cv2.putText(mapa, linea, + (pos_x, pos_y + i * 18), + cv2.FONT_HERSHEY_SIMPLEX, 0.48, color, 1, + cv2.LINE_AA) \ No newline at end of file diff --git a/out2.txt b/out2.txt new file mode 100644 index 0000000..e69de29 diff --git a/output_test.txt b/output_test.txt new file mode 100644 index 0000000..0f73b7d --- /dev/null +++ b/output_test.txt @@ -0,0 +1,3 @@ +Enviando comando de calibracion C: +Enviando comando A:1.000,1.000,1.000,1.000 (100% velocidad) +Stop enviado. diff --git a/planificador_astar.py b/planificador_astar.py new file mode 100644 index 0000000..6ee2cec --- /dev/null +++ b/planificador_astar.py @@ -0,0 +1,212 @@ +""" +planificador_astar.py +───────────────────── +Planificador de rutas A* para el robot. +Convierte el mapa SLAM (píxeles de obstáculos) en una cuadrícula +y calcula el camino óptimo evitando obstáculos. + +Integración con el proyecto: + from planificador_astar import PlanificadorAstar + + planificador = PlanificadorAstar(slam.mapa, escala=slam.escala, + centro=(slam.centro_x, slam.centro_y)) + ruta = planificador.planificar(robot.x, robot.y, + objetivo_x, objetivo_y) +""" + +import heapq +import math +import numpy as np +import cv2 +from typing import Optional + + +class PlanificadorAstar: + + def __init__(self, + mapa_slam: np.ndarray, + escala: float = 50.0, + centro: tuple = (400, 400), + margen_obstaculo: int = 8): + """ + Parameters + ---------- + mapa_slam : np.ndarray (H×W×3) — el mapa del AlgoritmoSLAM + escala : píxeles por metro (mismo valor que AlgoritmoSLAM.escala) + centro : (cx, cy) píxeles del origen del mapa + margen_obstaculo : radio de inflado de obstáculos en píxeles + (cuanto mayor, más separado pasa el robot) + """ + self.escala = escala + self.centro_x = centro[0] + self.centro_y = centro[1] + self.margen = margen_obstaculo + self.cuadricula: Optional[np.ndarray] = None + self._actualizar_cuadricula(mapa_slam) + + # ── API pública ────────────────────────────────────────────────────────── + + def planificar(self, + x_inicio: float, y_inicio: float, + x_meta: float, y_meta: float, + mapa_slam: Optional[np.ndarray] = None + ) -> list[tuple[float, float]]: + """ + Devuelve la lista de puntos (x_metros, y_metros) del camino, + desde la posición del robot hasta el objetivo. + Retorna [] si no existe camino. + """ + if mapa_slam is not None: + self._actualizar_cuadricula(mapa_slam) + + inicio = self._metros_a_celda(x_inicio, y_inicio) + meta = self._metros_a_celda(x_meta, y_meta) + + nodos_celdas = self._buscar(inicio, meta) + if not nodos_celdas: + return [] + + ruta_metros = [self._celda_a_metros(c) for c in nodos_celdas] + return ruta_metros + + def dibujar_ruta(self, + mapa: np.ndarray, + ruta_metros: list[tuple[float, float]], + color_camino: tuple = (0, 255, 255), + color_meta: tuple = (0, 165, 255), + color_waypoint: tuple = (200, 200, 0)) -> None: + """Pinta la ruta calculada sobre el mapa SLAM.""" + if not ruta_metros: + return + + puntos_px = [self._metros_a_px(x, y) for x, y in ruta_metros] + + # Línea del camino + for i in range(len(puntos_px) - 1): + cv2.line(mapa, puntos_px[i], puntos_px[i + 1], color_camino, 1) + + # Waypoints + for p in puntos_px[1:-1]: + cv2.circle(mapa, p, 2, color_waypoint, -1) + + # Meta + cv2.circle(mapa, puntos_px[-1], 6, color_meta, 2) + cv2.drawMarker(mapa, puntos_px[-1], color_meta, + cv2.MARKER_CROSS, 12, 1) + + # ── A* ─────────────────────────────────────────────────────────────────── + + def _buscar(self, + inicio: tuple[int, int], + meta: tuple[int, int] + ) -> list[tuple[int, int]]: + """Núcleo del algoritmo A* sobre la cuadrícula binaria.""" + h, w = self.cuadricula.shape + + def valida(r, c): + return 0 <= r < h and 0 <= c < w and self.cuadricula[r, c] == 0 + + if not valida(*inicio) or not valida(*meta): + print(f"⚠️ A*: inicio o meta dentro de un obstáculo. " + f"inicio={inicio} meta={meta}") + return [] + + # 8 vecinos (movimiento diagonal permitido) + VECINOS = [(-1,0),(1,0),(0,-1),(0,1), + (-1,-1),(-1,1),(1,-1),(1,1)] + COSTO = [1.0, 1.0, 1.0, 1.0, + 1.414, 1.414, 1.414, 1.414] + + def heuristica(a, b): + # Distancia octile (mejor que Manhattan para movimiento diagonal) + dr, dc = abs(a[0]-b[0]), abs(a[1]-b[1]) + return max(dr, dc) + (math.sqrt(2) - 1) * min(dr, dc) + + # (f, g, nodo) + abiertos: list = [] + heapq.heappush(abiertos, (0.0, 0.0, inicio)) + + g_score = {inicio: 0.0} + padres: dict = {} + cerrados: set = set() + + while abiertos: + f, g, actual = heapq.heappop(abiertos) + + if actual == meta: + return self._reconstruir(padres, actual) + + if actual in cerrados: + continue + cerrados.add(actual) + + for (dr, dc), costo in zip(VECINOS, COSTO): + vecino = (actual[0] + dr, actual[1] + dc) + if not valida(*vecino) or vecino in cerrados: + continue + + # Corte diagonal: no pasar por la esquina de un obstáculo + if dr != 0 and dc != 0: + if (self.cuadricula[actual[0]+dr, actual[1]] == 1 or + self.cuadricula[actual[0], actual[1]+dc] == 1): + continue + + g_nuevo = g + costo + if g_nuevo < g_score.get(vecino, float('inf')): + g_score[vecino] = g_nuevo + padres[vecino] = actual + f_nuevo = g_nuevo + heuristica(vecino, meta) + heapq.heappush(abiertos, (f_nuevo, g_nuevo, vecino)) + + print("⚠️ A*: no existe camino al objetivo") + return [] + + @staticmethod + def _reconstruir(padres: dict, + actual: tuple) -> list[tuple]: + ruta = [actual] + while actual in padres: + actual = padres[actual] + ruta.append(actual) + return list(reversed(ruta)) + + # ── Cuadrícula ─────────────────────────────────────────────────────────── + + def _actualizar_cuadricula(self, mapa_slam: np.ndarray) -> None: + """ + Convierte los píxeles blancos del mapa SLAM en celdas bloqueadas. + Aplica un margen (inflado morfológico) alrededor de cada obstáculo. + """ + gris = cv2.cvtColor(mapa_slam, cv2.COLOR_BGR2GRAY) + + # Píxeles cercanos al blanco → obstáculo + _, binario = cv2.threshold(gris, 180, 1, cv2.THRESH_BINARY) + + # Inflado: el robot no puede pasar cerca de paredes + if self.margen > 0: + kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (self.margen*2+1, self.margen*2+1)) + binario = cv2.dilate(binario, kernel, iterations=1) + + self.cuadricula = binario.astype(np.uint8) + + # ── Conversiones ───────────────────────────────────────────────────────── + + def _metros_a_celda(self, x: float, y: float) -> tuple[int, int]: + col = int(self.centro_x + x * self.escala) + fil = int(self.centro_y - y * self.escala) + h, w = self.cuadricula.shape + fil = max(0, min(fil, h - 1)) + col = max(0, min(col, w - 1)) + return (fil, col) + + def _celda_a_metros(self, celda: tuple[int, int]) -> tuple[float, float]: + fil, col = celda + x = (col - self.centro_x) / self.escala + y = (self.centro_y - fil) / self.escala + return (x, y) + + def _metros_a_px(self, x: float, y: float) -> tuple[int, int]: + px = int(self.centro_x + x * self.escala) + py = int(self.centro_y - y * self.escala) + return (px, py) diff --git a/scratch_test.cpp b/scratch_test.cpp new file mode 100644 index 0000000..372f934 --- /dev/null +++ b/scratch_test.cpp @@ -0,0 +1,10 @@ +#include +#include + +int main() { + std::string t = "A:0.027,0.027,0.027,0.027"; + float v[4] = {0, 0, 0, 0}; + int res = sscanf(t.c_str() + 2, "%f,%f,%f,%f", &v[0], &v[1], &v[2], &v[3]); + std::cout << "res=" << res << ", v0=" << v[0] << ", v1=" << v[1] << std::endl; + return 0; +} diff --git a/seguidor_ruta.py b/seguidor_ruta.py new file mode 100644 index 0000000..77a9b68 --- /dev/null +++ b/seguidor_ruta.py @@ -0,0 +1,74 @@ +import math +from typing import Tuple, List + +class SeguidorRuta: + """ + Controlador para que el robot siga una serie de puntos (ruta A*). + Utiliza un enfoque simplificado: gira hacia el waypoint y avanza. + """ + + def __init__(self, + umbral_distancia: float = 0.15, + vel_max: float = 0.50, + vel_giro: float = 1.50): + """ + Args: + umbral_distancia: Distancia en metros a la que se considera alcanzado un waypoint. + vel_max: Velocidad máxima de avance (m/s). + vel_giro: Velocidad máxima de giro (rad/s). + """ + self.umbral_distancia = umbral_distancia + self.vel_max = vel_max + self.vel_giro = vel_giro + + def calcular_velocidad(self, + x_robot: float, + y_robot: float, + theta_robot: float, + ruta: List[Tuple[float, float]]) -> Tuple[float, float, List[Tuple[float, float]]]: + """ + Calcula la velocidad lineal y angular para dirigirse al siguiente waypoint. + Retorna (v, w, ruta_actualizada). + """ + if not ruta: + return 0.0, 0.0, [] + + wx, wy = ruta[0] + + # Calcular distancia al waypoint actual + distancia = math.hypot(wy - y_robot, wx - x_robot) + + # Si estamos lo suficientemente cerca, pasar al siguiente punto + if distancia < self.umbral_distancia: + ruta.pop(0) + if not ruta: + # Llegamos al destino final + return 0.0, 0.0, [] + # Tomar las coordenadas del nuevo waypoint + wx, wy = ruta[0] + + # Calcular ángulo hacia el waypoint respecto al marco global + angulo_objetivo = math.atan2(wy - y_robot, wx - x_robot) + + # Calcular error de ángulo (relativo al robot) + error_angulo = angulo_objetivo - theta_robot + + # Normalizar el error al rango [-pi, pi] para girar por el lado más corto + error_angulo = (error_angulo + math.pi) % (2 * math.pi) - math.pi + + # Lógica de movimiento + # Si el error es grande (> 20 grados aprox), priorizar giro en el propio eje + umbral_giro = 0.35 + + if abs(error_angulo) > umbral_giro: + v = 0.0 + # Girar a la velocidad máxima en la dirección correspondiente + w = self.vel_giro if error_angulo > 0 else -self.vel_giro + else: + # Avanzar y corregir ligeramente la trayectoria (Control Proporcional suave) + v = self.vel_max + # Escala el giro suavemente pero sin superar el maximo + w = error_angulo * (self.vel_giro / umbral_giro) + w = max(-self.vel_giro, min(self.vel_giro, w)) + + return v, w, ruta diff --git a/sim_trama.cpp b/sim_trama.cpp new file mode 100644 index 0000000..2148277 --- /dev/null +++ b/sim_trama.cpp @@ -0,0 +1,47 @@ +#include +#include + +// Simulating Arduino String +class String { + std::string str; +public: + String(const char* s) : str(s) {} + int length() const { return str.length(); } + char charAt(int i) const { return str[i]; } + int indexOf(char c, int start) const { + size_t pos = str.find(c, start); + return pos == std::string::npos ? -1 : pos; + } + String substring(int start, int end) const { + return String(str.substr(start, end - start).c_str()); + } + String substring(int start) const { + return String(str.substr(start).c_str()); + } + float toFloat() const { + return std::stof(str); + } + const char* c_str() const { return str.c_str(); } +}; + +int main() { + String t = "A:0.265,0.265,0.265,0.265"; + float v[4] = {0, 0, 0, 0}; + + int c1 = t.indexOf(',', 2); + int c2 = t.indexOf(',', c1 + 1); + int c3 = t.indexOf(',', c2 + 1); + + if (c1 == -1 || c2 == -1 || c3 == -1) { + std::cout << "Parse failed" << std::endl; + return 1; + } + + v[0] = t.substring(2, c1).toFloat(); + v[1] = t.substring(c1 + 1, c2).toFloat(); + v[2] = t.substring(c2 + 1, c3).toFloat(); + v[3] = t.substring(c3 + 1).toFloat(); + + std::cout << "v0: " << v[0] << ", v1: " << v[1] << ", v2: " << v[2] << ", v3: " << v[3] << std::endl; + return 0; +} diff --git a/teleop.py b/teleop.py new file mode 100644 index 0000000..6265133 --- /dev/null +++ b/teleop.py @@ -0,0 +1,90 @@ +import sys +import select +import tty +import termios +from modulo_esp32 import ChasisESP32 + +# Mensaje de interfaz +MSG = """ +Control Manual del Robot (Modo Python Puro) +------------------------------------------- +Controles de movimiento: + w + a s d + +Espacio o 'x' : FRENAR DE GOLPE +'q' : SALIR Y APAGAR MOTORES + +Aumentos por pulsación: +w/s : +- 0.05 m/s (Velocidad Lineal) +a/d : +- 0.10 rad/s (Velocidad Angular) +""" + +# Límites de seguridad para que el robot no salga volando +VEL_LINEAL_MAX = 0.5 # Metros por segundo +VEL_ANGULAR_MAX = 1.0 # Radianes por segundo +PASO_LINEAL = 0.05 +PASO_ANGULAR = 0.1 + +def obtener_tecla(settings): + """Magia de Linux para leer una sola tecla sin presionar Enter""" + tty.setraw(sys.stdin.fileno()) + select.select([sys.stdin], [], [], 0) + key = sys.stdin.read(1) + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + +def main(): + # Guardar la configuración de la terminal para restaurarla al final + settings = termios.tcgetattr(sys.stdin) + + # Inicializar la conexión UART con la ESP32 + print("Iniciando conexión con el chasis...") + robot = ChasisESP32(puerto='/dev/ttyAMA0') + + v = 0.0 + w = 0.0 + + try: + print(MSG) + while True: + tecla = obtener_tecla(settings) + + # Lógica de movimiento + if tecla == 'w': + v += PASO_LINEAL + elif tecla == 's': + v -= PASO_LINEAL + elif tecla == 'a': + w += PASO_ANGULAR # Giro a la izquierda (positivo en regla de mano derecha) + elif tecla == 'd': + w -= PASO_ANGULAR # Giro a la derecha + elif tecla == ' ' or tecla == 'x': + v = 0.0 + w = 0.0 + elif tecla == 'q': + break + + # Aplicar límites de seguridad + v = max(min(v, VEL_LINEAL_MAX), -VEL_LINEAL_MAX) + w = max(min(w, VEL_ANGULAR_MAX), -VEL_ANGULAR_MAX) + + # Imprimir estado actual sobrescribiendo la misma línea + print(f"\r🚀 Velocidad -> Lineal: {v:.2f} m/s | Angular: {w:.2f} rad/s ", end='') + + # Enviar el comando matemático a la ESP32 + robot.enviar_velocidad(v, w) + + except Exception as e: + print(f"\n❌ Error en teleop: {e}") + + finally: + # 1. Por seguridad, frenar el robot al salir + print("\nDeteniendo motores...") + robot.enviar_velocidad(0.0, 0.0) + # 2. Restaurar la terminal a su estado normal + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + print("Teleop cerrado exitosamente.") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/test_motor.py b/test_motor.py new file mode 100644 index 0000000..fcee43f --- /dev/null +++ b/test_motor.py @@ -0,0 +1,12 @@ +import serial +import time + +try: + puerto = serial.Serial('/dev/ttyAMA0', 921600, timeout=1.0) + print("Enviando A:0.500,0.500,0.500,0.500") + puerto.write(b"A:0.500,0.500,0.500,0.500\n") + time.sleep(2) + puerto.write(b"S:\n") + print("Stop enviado.") +except Exception as e: + print("Error:", e) diff --git a/test_uart.py b/test_uart.py new file mode 100644 index 0000000..5fa8a19 --- /dev/null +++ b/test_uart.py @@ -0,0 +1,26 @@ +import serial +import time + +try: + puerto = serial.Serial('/dev/ttyAMA0', 921600, timeout=0.1) + print("Puerto abierto correctamente.") + + # Limpiar buffer + puerto.reset_input_buffer() + + # Enviar comando de adelante al 20% + print("Enviando comando: A:0.200,0.200,0.200,0.200") + puerto.write(b"A:0.200,0.200,0.200,0.200\n") + + t_end = time.time() + 2.0 + while time.time() < t_end: + if puerto.in_waiting: + line = puerto.readline().decode('utf-8', errors='ignore').strip() + print("RX:", line) + + # Detener + puerto.write(b"S:\n") + print("Comando S (stop) enviado.") + +except Exception as e: + print("Error:", e) diff --git a/test_uart2.py b/test_uart2.py new file mode 100644 index 0000000..54ea4fb --- /dev/null +++ b/test_uart2.py @@ -0,0 +1,14 @@ +import serial +import time + +try: + puerto = serial.Serial('/dev/ttyAMA0', 921600, timeout=0.5) + print("Puerto abierto.") + puerto.reset_input_buffer() + + for _ in range(10): + raw = puerto.readline() + print("Raw:", raw) + +except Exception as e: + print("Error:", e) diff --git a/test_uart_direct.py b/test_uart_direct.py new file mode 100644 index 0000000..14c8b92 --- /dev/null +++ b/test_uart_direct.py @@ -0,0 +1,40 @@ +import serial +import time + +try: + puerto = serial.Serial('/dev/ttyAMA0', 921600, timeout=1.0) + print("Enviando comando de calibracion C:") + puerto.write(b"C:\n") + time.sleep(1) + + # Leer todo lo que devuelva + lines = [] + end_time = time.time() + 2 + while time.time() < end_time: + if puerto.in_waiting: + lines.append(puerto.readline().decode('utf-8', errors='ignore').strip()) + + for l in lines[-10:]: # Mostrar ultimas 10 lineas para no saturar + print("RX:", l) + + print("Enviando comando A:1.000,1.000,1.000,1.000 (100% velocidad)") + puerto.write(b"A:1.000,1.000,1.000,1.000\n") + time.sleep(0.5) + + # Leer telemetria para ver si incrementan los ticks + ticks_lines = [] + end_time = time.time() + 1 + while time.time() < end_time: + if puerto.in_waiting: + line = puerto.readline().decode('utf-8', errors='ignore').strip() + if line.startswith("T:"): + ticks_lines.append(line) + + for l in ticks_lines[-5:]: + print("TICKS:", l) + + puerto.write(b"S:\n") + print("Stop enviado.") + +except Exception as e: + print("Error:", e) diff --git a/test_uart_raw.py b/test_uart_raw.py new file mode 100644 index 0000000..12be22f --- /dev/null +++ b/test_uart_raw.py @@ -0,0 +1,13 @@ +import serial +import time + +try: + puerto = serial.Serial('/dev/ttyAMA0', 921600, timeout=1.0) + print("Escuchando...") + t_end = time.time() + 2.0 + while time.time() < t_end: + if puerto.in_waiting: + print("Bytes:", puerto.read(puerto.in_waiting)) + print("Fin.") +except Exception as e: + print("Error:", e)