self_driving_car.py

from picamera.array import PiRGBArray
import RPi.GPIO as GPIO
from picamera import PiCamera
import time
import cv2
import numpy as np
import math

GPIO.setmode(GPIO.BCM)
GPIO.setup(13, GPIO.OUT)
GPIO.setup(19, GPIO.OUT)
GPIO.setup(5, GPIO.OUT)
GPIO.setup(12, GPIO.OUT)

GPIO.output(5, GPIO.LOW)
GPIO.output(12, GPIO.LOW)
GPIO.output(19, GPIO.LOW)
GPIO.output(13, GPIO.LOW)

theta=0
minLineLength = 5
maxLineGap = 10

camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 15

rawCapture = PiRGBArray(camera, size=(640, 480))
time.sleep(0.1)

for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
   GPIO.output(5, GPIO.LOW)
   GPIO.output(12, GPIO.LOW)
   GPIO.output(13, GPIO.LOW)
   GPIO.output(19, GPIO.LOW)
   time.sleep(0.0)
   image = frame.array
   gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
   blurred = cv2.GaussianBlur(gray, (5, 5), 0)
   edged = cv2.Canny(blurred, 85, 85)
   lines = cv2.HoughLinesP(edged,1,np.pi/180,10,minLineLength,maxLineGap)
   
   if(lines !=[]):
       for x in range(0, len(lines)):
           for x1,y1,x2,y2 in lines[x]:
               cv2.line(image,(x1,y1),(x2,y2),(0,255,0),2)
               theta=theta+math.atan2((y2-y1),(x2-x1))
          
   threshold=6
   
   if(theta>threshold):
       GPIO.output(5, GPIO.LOW)
       GPIO.output(12, GPIO.HIGH)
       GPIO.output(19, GPIO.LOW)
       GPIO.output(13, GPIO.HIGH)
       print("left")
       
   if(theta<-threshold):
       GPIO.output(12, GPIO.LOW)
       GPIO.output(5, GPIO.HIGH)
       GPIO.output(13, GPIO.LOW)
       GPIO.output(19, GPIO.HIGH)
       print("right")
       
   if(abs(theta)<threshold):
      GPIO.output(5, GPIO.LOW)
      GPIO.output(12, GPIO.HIGH)
      GPIO.output(13, GPIO.LOW)
      GPIO.output(19, GPIO.HIGH)
      print ("straight")
   
   theta=0
   cv2.imshow("Frame",image)
   key = cv2.waitKey(1) & 0xFF
   rawCapture.truncate(0)
   
   if key == ord("q"):
       break