# pip install opencv-python opencv-contrib-python scipy # Comando para instalar las librerías necesarias de visión por computadora import time # Importamos la librería time para manejar tiempos de espera import cv2 # Importamos la librería OpenCV, que nos permite trabajar con imágenes y video import matplotlib.pyplot as plt # Importamos matplotlib para graficar import numpy as np # Importamos NumPy para trabajar con arreglos numéricos from scipy.ndimage import center_of_mass # Importamos la función center_of_mass para calcular el centro de masa de un arreglo import ev3_dc as ev3 # Importamos la librería ev3_dc para controlar el robot EV3 from thread_task import Periodic # Importamos Periodic para crear tareas periódicas import keyboard # Importamos keyboard para capturar entradas del teclado # Dirección MAC del ladrillo EV3 MAC = '00:16:53:50:8A:86' # Cambia esto por la dirección MAC de tu ladrillo EV3 # Cambia esta dirección IP por la de tu cámara cap = cv2.VideoCapture('http://192.168.137.3:8080/video') # Creamos un objeto que captura video desde una cámara IP # VideoCapture abre la conexión a la dirección web de la cámara # Configuramos matplotlib para modo interactivo plt.ion() # Creamos una figura y un eje para graficar fig, ax = plt.subplots() # Ajustamos el tamaño de la figura ax.figure.set_size_inches(5, 5) ax.set_title('Intensidad de la línea horizontal central') ax.set_xlim(0, 480) # Asumiendo un ancho de imagen de 480 píxeles ax.set_ylim(0, 255) # Valores de intensidad en escala de grises intensity_plot = ax.plot([], 'r-')[0] center_x_plot = ax.plot([], 'ko')[0] fig.canvas.draw() vt = 0 vr = 0 vd = 0 vi = 0 speed = 0 turn = 0 vt_new = 0.0 # Velocidad translacional constante # Constante proporcional para el control P Kp = 0.05 def change_translational_speed(new_vt): global vt_new vt_new += new_vt vt_new = max(min(vt_new, 50), -50) print(f"Translational speed set to: {vt_new}") def change_Kp(new_Kp): global Kp Kp += new_Kp print(f"Kp set to: {Kp}") keyboard.add_hotkey('up', lambda: change_translational_speed(5)) keyboard.add_hotkey('down', lambda: change_translational_speed(-5)) keyboard.add_hotkey('left', lambda: change_Kp(0.01)) keyboard.add_hotkey('right', lambda: change_Kp(-0.01)) print("Use arrow keys to adjust translational speed and Kp:") print("Up/Down: Increase/Decrease translational speed") print("Left/Right: Increase/Decrease Kp") def get_turn(vmax, vmin, sign=1): if vmax == 0: return 0 ratio = 100 * (1 - vmin / vmax) return int(sign * ratio) def update_speed_turn(vt_new, vr_new): global vt, vr, speed, turn, vd, vi vt = max(min(int(vt_new), 50), -50) vr = max(min(int(vr_new), 50), -50) vd = vt + vr vi = vt - vr if abs(vd) > abs(vi): speed = max(min(vd, 100), -100) turn = get_turn(vd, vi, 1) else: speed = max(min(vi, 100), -100) turn = get_turn(vi, vd, -1) with ev3.TwoWheelVehicle( 0.0210, # radius_wheel 0.1224, # tread protocol=ev3.BLUETOOTH, host=MAC, speed=40 ) as vehicle: def drive(): global speed, turn if speed != 0: vehicle.move(speed, turn) else: vehicle.stop(brake=True) drive_task = Periodic(0.1, drive) drive_task.start() # Iniciamos un bucle que se ejecuta mientras la cámara esté abierta while (cap.isOpened()): # Leemos un frame (fotograma) de la cámara # ret indica qsi la lectura fue exitosa, frame contiene la imagen # Drop frames to get the latest one for _ in range(5): cap.grab() ret, frame = cap.read() # Rotamos el frame 90 grados en sentido horario frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) # Convertimos el frame de BGR (Azul, Verde, Rojo) a escala de grises gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Aplicamos un filtro gaussiano para reducir el ruido # gray = cv2.GaussianBlur(gray, (10, 10), 0) # Obtenemos las dimensiones de la imagen en escala de grises height, width = gray.shape # Estraemos el vector de valores de intensidad de la línea horizontal central horizontal_line = gray[height // 2, :] horizontal_line_mean = horizontal_line.mean() umbral = horizontal_line_mean * 0.7 pesos = np.maximum(0, umbral - horizontal_line) centro_x = center_of_mass(pesos)[0] centro_x = centro_x if not np.isnan(centro_x) else width // 2 # Calculamos el error como la diferencia entre el centro de la imagen y el centro de masa error_x = (width / 2) - centro_x # Calculamos la velocidad rotacional basada en el error vr_new = Kp * error_x update_speed_turn(vt_new, vr_new) # Ploteamos la intensidad de la línea horizontal central en la imagen intensity_plot.set_xdata(range(len(horizontal_line))) intensity_plot.set_ydata(horizontal_line) center_x_plot.set_xdata([centro_x]) center_x_plot.set_ydata([horizontal_line[int(centro_x)]]) fig.canvas.flush_events() # Dibujamos una linea vertical en el centro de la imagen cv2.line(frame, (width // 2, 0), (width // 2, height), (0, 0, 200), 2) # Dibujamos una línea horizontal en el centro de la imagen cv2.line(frame, (0, height // 2), (width, height // 2), (0, 0, 255), 2) # Dibujamos un círculo en el centro de masa calculado cv2.circle(frame, (int(centro_x), height // 2), 8, (0, 0, 255), -1) cv2.circle(frame, (int(centro_x), height // 2), 5, (0, 0, 0), -1) try: # Mostramos el frame en una ventana llamada 'Camara' # cv2.resize() redimensiona la imagen a 480x640 píxeles cv2.imshow('Camara', cv2.resize(frame, (480, 640))) # Capturamos la entrada del teclado (espera 1ms por cada iteración) key = cv2.waitKey(1) # Si presionamos 'q', salimos del bucle if key == ord('q'): break # Capturamos excepciones de OpenCV por si la transmisión se corta except cv2.error: print("Stream ended...") break drive_task.stop() vehicle.stop(brake=True) time.sleep(1) # Wait for the vehicle to stop print("Programa terminado") # Liberamos los recursos de la cámara cap.release() # Cerramos todas las ventanas creadas por OpenCV cv2.destroyAllWindows()