90 lines
2.7 KiB
Python
90 lines
2.7 KiB
Python
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() |