diff --git a/main.py b/main.py index 1a24330..ed9ca0e 100644 --- a/main.py +++ b/main.py @@ -9,9 +9,9 @@ from modulo_slam import EvasorObstaculos from modulo_imu import IMU # ── 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 +VEL_NORMAL = 0.65 # m/s en camino libre +VEL_LENTA = 0.40 # m/s en precaución +VEL_GIRO = 2 # rad/s al girar # ── Mapa 2D local ────────────────────────────────────────────────────────────── MAPA_W, MAPA_H = 700, 700 @@ -282,9 +282,15 @@ def main(): estado_auto = AUTO_LIBRE # estado actual de la máquina theta_inicio = 0.0 # theta del IMU al iniciar el paso de giro w_giro_actual = 0.0 # dirección del giro en curso (+/-) + incremento_w = 0.0 # compensación dinámica de giro por fricción + ultimo_tiempo_auto = time.perf_counter() try: while True: + ahora = time.perf_counter() + dt = ahora - ultimo_tiempo_auto + ultimo_tiempo_auto = ahora + # Cámara: solo se usa para el evasor de obstáculos, sin mostrar video escaner, _ = camara.obtener_datos() @@ -321,11 +327,25 @@ def main(): # Completó ~30° → detener y evaluar robot.enviar_velocidad(0.0, 0.0) estado_auto = AUTO_CHECK + incremento_w = 0.0 print(f"\n✓ Giro completado ({math.degrees(girado):.0f}°). Evaluando camino...") else: + # ── Compensación de fricción ── + # Si el robot está atascado (IMU marca casi cero giro), aumentamos el PWM + w_real = abs(imu.omega_x_rads) + if w_real < 0.2: + incremento_w += 1.5 * dt # Rampa de subida (1.5 rad/s por segundo) + else: + incremento_w -= 3.0 * dt # Rampa de bajada más rápida si ya gira + + # Limitar entre 0 y 2.5 rad/s extra para que no se vuelva loco + incremento_w = max(0.0, min(incremento_w, 2.5)) + + w_compensado = w_giro_actual + math.copysign(incremento_w, w_giro_actual) + # Continuar girando - robot.enviar_velocidad(0.0, w_giro_actual) - print(f"\r🔄 Girando... {math.degrees(girado):.0f}° / 30°", end="") + robot.enviar_velocidad(0.0, w_compensado) + print(f"\r🔄 Girando... {math.degrees(girado):.0f}° / 30° (Boost: {incremento_w:.1f})", end="") elif estado_auto == AUTO_CHECK: # ── Verificar si el camino quedó libre ──────────────────── @@ -369,12 +389,14 @@ def main(): estado_auto = AUTO_LIBRE theta_inicio = robot.theta w_giro_actual = 0.0 + incremento_w = 0.0 modo_autonomo = True elif tecla == ord('m'): print("\n🛑 MODO MANUAL") modo_autonomo = False estado_auto = AUTO_LIBRE + incremento_w = 0.0 robot.enviar_velocidad(0.0, 0.0) elif tecla == ord('c'):