Final Lab de Control
Código para detectar carril y control de carro autonomo
unknown
python
3 years ago
7.2 kB
6
Indexable
# OpenCV program to perform Edge detection in real time by Jonathan Gaona
# Proyecto Final Lab de Control
# import libraries of python OpenCV
import cv2
import RPi.GPIO as GPIO
import numpy as np
import time
from time import sleep
prev_frame_time=0
new_frame_time=0
in1 = 26 #Puente H entradas de control
in2 = 19
enA = 11
in4 = 13
in3 = 6
enB = 5
GPIO.setmode(GPIO.BCM)
GPIO.setup(in1,GPIO.OUT) #inicializacion de las salidas
GPIO.setup(in2,GPIO.OUT)
GPIO.setup(enA,GPIO.OUT)
GPIO.setup(in3,GPIO.OUT)
GPIO.setup(in4,GPIO.OUT)
GPIO.setup(enB,GPIO.OUT)
GPIO.output(in1,GPIO.LOW)
GPIO.output(in2,GPIO.LOW)
GPIO.output(in3,GPIO.LOW)
GPIO.output(in4,GPIO.LOW)
p=GPIO.PWM(enA,1000)
p2=GPIO.PWM(enB,1000)
p.start(36)
p2.start(36)
#FUNCIONES para encontrar el centro entre las lineas de Hough
def findCenter(p1,p2):
center = ((p1[0] + p2[0]) // 2, (p1[1] + p2[1]) // 2)
return center
def minmax_centerPoints(tergetList,pos):
if len(tergetList) > 0:
maximum = max(tergetList, key = lambda i: i[pos])
minimum = min(tergetList, key = lambda i: i[pos])
return [maximum,minimum]
else:
return None
#Funcion principal del procesamiento de imagen
def detectedlane(imageFrame):
center1= 0
center2 = 0
cv2.imshow('Original',imageFrame) #mostrar imagen original
width,height = 360,180
pts1 = [[0,160],[320,240],[300,30],[20,20]]
pts2 = [[0, height], [width, height],[width,0], [0,0]]
target = np.float32(pts1)
destination = np.float32(pts2)
#Algoritmo de transformacion de perspectiva
matrix = cv2.getPerspectiveTransform(target, destination)
result = cv2.warpPerspective(frame, matrix, (width,height))
cv2.imshow('Perspectiva', result) #mostrar ajuste de perspectiva
gray = cv2.cvtColor(result, cv2.COLOR_BGR2GRAY) #cambiar a blanco y negro
threshold = cv2.inRange(gray, 190,300 )#metodo de umbralizacion
cv2.imshow('Blanco y Negro',gray) #mostrar imagen blanco y negro
cv2.imshow('Threshold',threshold) #mostrar imagen threshold
#creacion de la linea horizontal roja de la perspectiva
firstSquareCenters1 = findCenter((pts2[1][0], pts2[1][1]), (pts2[2][0], pts2[2][1]))
firstSquareCenters2 = findCenter((pts2[3][0], pts2[3][1]), (pts2[0][0], pts2[0][1]))
cv2.line(result, firstSquareCenters1, firstSquareCenters2, (0, 0, 255), 1)
mainFrameCenter = findCenter(firstSquareCenters1,firstSquareCenters2)
#Metodo de lineas de Hough
lines = cv2.HoughLinesP(threshold, 1, np.pi/180, 90,minLineLength=50, maxLineGap=450)
centerPoints = []
left = []
right = []
if lines is not None:
for line in lines:
x1,y1,x2,y2 = line[0]
cv2.line(result, (x1, y1), (x2, y2), (0, 255, 0), 5)
if 0<=x1 <=width and 0<= x2 <=width :
center = findCenter((x1,y1),(x2,y2))
if center[0] < (width//2):
center1 = center
left.append((x1, y1))
left.append((x2,y2))
else:
center2 = center
right.append((x1, y1))
right.append((x2,y2))
if center1 !=0 and center2 !=0:
centroid1 = findCenter(center1,center2)
centerPoints.append(centroid1)
centers = minmax_centerPoints(centerPoints,1)
laneCenters = 0
mainCenterPosition = 0
if centers is not None:
laneframeCenter = findCenter(centers[0],centers[1])
#print(mainFrameCenter,laneframeCenter)
mainCenterPosition = mainFrameCenter[0] - laneframeCenter[0]
laneCenters = centers
return[laneCenters,result,mainCenterPosition]
# Capturar video de la camara
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,320) # set the width to 320 p
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,240) # set the height to 240 p
#Variables para controlar
maincenter=0
kp = 1.35
kd = 0.025
ki = 0.02
kp2 = 1.35
kd2 = 0.025
ki2 = 0.02
e1prev=0
e2prev=0
e1sum=0
e2sum=0
pwm=0
pwm2=0
fps=0
# loop runs if capturing has been initialized
while(1):
# reads frames from a camera
ret, frame = cap.read()
if ret == True:
# Se llama la funcion de procesamiento de imagen
laneimage1 = detectedlane(frame)
if laneimage1 is not None:
maincenter = laneimage1[2]
lcenter=laneimage1[0]
cv2.putText(laneimage1[1],"Pos="+str(maincenter),(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255))
cv2.putText(laneimage1[1],"Fps="+str(fps),(290, 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255))
cv2.imshow('FinalWindow',laneimage1[1]) #mostrar imagen final
else:
cv2.imshow('FinalWindow', frame)
#limites de velocidad
limit1=64
limit2=-64
#error (angulo)
error=maincenter
error2=maincenter
#incrementar el limite de velocidad
if error>25:
limit1=70
elif error>40:
limit1=75
if error2<-23:
limit2=-70
elif error2<-40:
limit2=-75
#Controlador PID
pwm += (error * kp) + (e1prev*kd)+(e1sum*ki)
if pwm>=limit1:
pwm=limit1
elif pwm<0:
pwm=10
#PID 2
pwm2 += (error2 * kp2) + (e2prev*kd2)+(e2sum*ki2)
if pwm2<=limit2:
pwm2=limit2
elif pwm2>-10:
pwm2=-10
#Se mantiene un margen donde se considera que va en la
# trayectoria correcta
if maincenter <=6 and maincenter >= -6:
#print("Sigue Derecho")
GPIO.output(in1,GPIO.HIGH)
GPIO.output(in2,GPIO.LOW)
GPIO.output(in3,GPIO.HIGH)
GPIO.output(in4,GPIO.LOW)
p.ChangeDutyCycle(37)
p2.ChangeDutyCycle(37)
elif maincenter >6:
p.ChangeDutyCycle(int(abs(pwm2)))
p2.ChangeDutyCycle(int(abs(pwm)))
#A Lazo cerrado solo se usa la velocidad fija
#p.ChangeDutyCycle(10)
#p2.ChangeDutyCycle(limit1)
else:
p.ChangeDutyCycle(int(abs(pwm2)))
p2.ChangeDutyCycle(int(abs(pwm)))
#p.ChangeDutyCycle(abs(limit2))
#p2.ChangeDutyCycle(10)
#Variables para el controlador
e1prev=error
e2prev=error
e1sum += error
e2sum += error
#calculo de fps
new_frame_time=time.time()
fps=1/(new_frame_time-prev_frame_time)
prev_frame_time=new_frame_time
fps=int(fps)
# al presionar tecla ESC se termina el programa
k = cv2.waitKey(5) & 0xFF
if k == 27:
GPIO.cleanup()
break
# Close the window
cap.release()
# De-allocate any associated memory usage
cv2.destroyAllWindows()Editor is loading...