diff --git a/v1.0.0/requirements.txt b/v1.0.0/requirements.txt new file mode 100644 index 0000000..14b6b35 --- /dev/null +++ b/v1.0.0/requirements.txt @@ -0,0 +1,3 @@ +numpy==2.2.0 +opencv-python==4.10.0.84 +RPi.GPIO==0.7.1 diff --git a/v1.0.0/v1.0.0.py b/v1.0.0/v1.0.0.py new file mode 100644 index 0000000..3f89260 --- /dev/null +++ b/v1.0.0/v1.0.0.py @@ -0,0 +1,139 @@ +import cv2 +import numpy as np +import RPi.GPIO as GPIO +import time + +# Configuración de pines GPIO para los motores +class MotorController: + def __init__(self): + # Configurar modo de numeración GPIO + GPIO.setmode(GPIO.BCM) + + # Definir pines para los motores + self.motor_izq_adelante = 17 + self.motor_izq_atras = 18 + self.motor_der_adelante = 22 + self.motor_der_atras = 23 + + # Configurar pines como salida + GPIO.setup(self.motor_izq_adelante, GPIO.OUT) + GPIO.setup(self.motor_izq_atras, GPIO.OUT) + GPIO.setup(self.motor_der_adelante, GPIO.OUT) + GPIO.setup(self.motor_der_atras, GPIO.OUT) + + # Inicializar PWM para control de velocidad + self.pwm_izq = GPIO.PWM(self.motor_izq_adelante, 100) + self.pwm_der = GPIO.PWM(self.motor_der_adelante, 100) + self.pwm_izq.start(0) + self.pwm_der.start(0) + + def mover(self, velocidad_izq, velocidad_der): + """ + Control de movimiento de los motores + velocidad: -100 a 100 (negativo para reversa) + """ + # Motor izquierdo + if velocidad_izq >= 0: + self.pwm_izq.ChangeDutyCycle(velocidad_izq) + GPIO.output(self.motor_izq_atras, GPIO.LOW) + else: + self.pwm_izq.ChangeDutyCycle(-velocidad_izq) + GPIO.output(self.motor_izq_atras, GPIO.HIGH) + + # Motor derecho + if velocidad_der >= 0: + self.pwm_der.ChangeDutyCycle(velocidad_der) + GPIO.output(self.motor_der_atras, GPIO.LOW) + else: + self.pwm_der.ChangeDutyCycle(-velocidad_der) + GPIO.output(self.motor_der_atras, GPIO.HIGH) + + def detener(self): + """Detiene ambos motores""" + self.pwm_izq.ChangeDutyCycle(0) + self.pwm_der.ChangeDutyCycle(0) + GPIO.output(self.motor_izq_atras, GPIO.LOW) + GPIO.output(self.motor_der_atras, GPIO.LOW) + +def procesar_frame(frame): + """ + Procesa el frame y detecta la línea negra + Retorna: frame procesado y posición de la línea + """ + # Convertir a escala de grises + gris = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # Aplicar desenfoque para reducir ruido + desenfoque = cv2.GaussianBlur(gris, (5, 5), 0) + + # Binarización para detectar la línea negra + _, binario = cv2.threshold(desenfoque, 60, 255, cv2.THRESH_BINARY_INV) + + # Región de interés (parte inferior de la imagen) + altura = frame.shape[0] + roi = binario[int(altura*0.6):altura, :] + + # Encontrar el centro de la línea + momentos = cv2.moments(roi) + if momentos["m00"] > 0: + cx = int(momentos["m10"] / momentos["m00"]) + return binario, cx + else: + return binario, None + +def seguidor_linea(): + # Inicializar cámara + camara = cv2.VideoCapture(0) + camara.set(cv2.CAP_PROP_FRAME_WIDTH, 320) + camara.set(cv2.CAP_PROP_FRAME_HEIGHT, 240) + + # Inicializar controlador de motores + motores = MotorController() + + # Parámetros de control + velocidad_base = 50 # Velocidad base de los motores + kp = 0.5 # Constante proporcional para el control + + try: + while True: + ret, frame = camara.read() + if not ret: + print("Error al capturar frame") + break + + # Procesar frame y obtener posición de la línea + frame_procesado, centro_linea = procesar_frame(frame) + + if centro_linea is not None: + # Calcular error (distancia desde el centro) + error = frame.shape[1]/2 - centro_linea + + # Calcular ajuste de velocidad usando control proporcional + ajuste = kp * error + + # Aplicar velocidades a los motores + velocidad_izq = max(min(velocidad_base + ajuste, 100), -100) + velocidad_der = max(min(velocidad_base - ajuste, 100), -100) + + motores.mover(velocidad_izq, velocidad_der) + else: + # Si no se detecta la línea, detener motores + motores.detener() + + # Mostrar frames para debugging + cv2.imshow('Video Original', frame) + cv2.imshow('Línea Detectada', frame_procesado) + + # Salir con 'q' + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + finally: + # Limpieza + motores.detener() + camara.release() + cv2.destroyAllWindows() + GPIO.cleanup() + +if __name__ == "__main__": + seguidor_linea()