140 lines
4.4 KiB
Python
140 lines
4.4 KiB
Python
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()
|