Funciona todo sin modulo_imu
This commit is contained in:
@@ -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)); }
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
+192
@@ -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}")
|
||||
+296
@@ -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,
|
||||
}
|
||||
+196
@@ -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)
|
||||
@@ -0,0 +1,3 @@
|
||||
Enviando comando de calibracion C:
|
||||
Enviando comando A:1.000,1.000,1.000,1.000 (100% velocidad)
|
||||
Stop enviado.
|
||||
@@ -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)
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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()
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user