Juan/RoboticaPython
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Funciona todo sin modulo_imu

Browse Source
This commit is contained in:
Juan
2026-05-21 09:51:42 -05:00
parent 026820f205
commit 19dc4bcf90
19 changed files with 1857 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
AutoCalibracion/AutoCalibracion.ino
+395
View File
@@ -0,0 +1,395 @@
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/task.h"
#include <Arduino.h>
#include <cmath>
// ==========================================
// 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)); }
a.out
Executable
BIN
View File
Binary file not shown.
main.py
+208
View File
@@ -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()
modulo_camara.py
+29
View File
@@ -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()
modulo_esp32.py
+192
View File
@@ -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}")
modulo_imu.py
+296
View File
@@ -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,
}
modulo_slam.py
+196
View File
@@ -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)
out2.txt
View File
output_test.txt
+3
View File
@@ -0,0 +1,3 @@
Enviando comando de calibracion C:
Enviando comando A:1.000,1.000,1.000,1.000 (100% velocidad)
Stop enviado.
planificador_astar.py
+212
View File
@@ -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)
scratch_test.cpp
+10
View File
@@ -0,0 +1,10 @@
#include <iostream>
#include <string>
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;
}
seguidor_ruta.py
+74
View File
@@ -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
sim_trama.cpp
+47
View File
@@ -0,0 +1,47 @@
#include <iostream>
#include <string>
// 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;
}
teleop.py
+90
View File
@@ -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()
test_motor.py
+12
View File
@@ -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)
test_uart.py
+26
View File
@@ -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)
test_uart2.py
+14
View File
@@ -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)
test_uart_direct.py
+40
View File
@@ -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)
test_uart_raw.py
+13
View File
@@ -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)