MrLunk

Raspberry Pi - Python2 - OpenCV3 Line following rover basics

May 22nd, 2017
1,783
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. # OpenCV Line following rover basics Py2Cv3
  2. # Will be used and demonstrated with this Vehikel soon: https://www.youtube.com/watch?v=7wFg_FFjx6Y
  3.  
  4. import numpy as np
  5. import cv2
  6. import time
  7.  
  8. video_capture = cv2.VideoCapture(0)
  9. video_capture.set(3, 640)
  10. video_capture.set(4, 480)
  11.  
  12. # ---- Function definition for converting scales ------
  13. def remap(unscaled, to_min, to_max, from_min, from_max):
  14.     return (to_max-to_min)*(unscaled-from_min)/(from_max-from_min)+to_min
  15.  
  16. # ---- Main loop ----
  17. while(True):
  18.  
  19.     # Capture from camera
  20.     ret, frame = video_capture.read()
  21.  
  22.     # Crop, select part of image to work with
  23.     crop_img = frame[380:480, 0:640]
  24.  
  25.     # Make the image greyscale
  26.     gray = cv2.cvtColor(crop_img, cv2.COLOR_BGR2GRAY)
  27.  
  28.     # uncomment next line to view greyscale image
  29.     #cv2.imshow('Gray',gray)
  30.  
  31.     # Apply a Gaussian blur
  32.     blur = cv2.GaussianBlur(gray,(5,5),0)
  33.  
  34.     # uncomment next line to view Blurred image
  35.     #cv2.imshow('Blur',blur)
  36.  
  37.     # Apply Color thresholding
  38.     ret,thresh = cv2.threshold(blur,100,255,cv2.THRESH_BINARY_INV)
  39.  
  40.     # uncomment next line to view Threshholded image    
  41.     #cv2.imshow('Thresh',thresh)
  42.  
  43.     # Find the contours in the cropped image part
  44.     img, contours, hierarchy = cv2.findContours(thresh.copy(), 1, cv2.CHAIN_APPROX_NONE)
  45.  
  46.     # ---------------- Find the biggest contour = line -----------------
  47.    
  48.     if len(contours) > 0:
  49.         c = max(contours, key=cv2.contourArea)
  50.         M = cv2.moments(c)
  51.  
  52.         cx = int(M['m10']/M['m00'])
  53.         cy = int(M['m01']/M['m00'])
  54.  
  55.         cv2.line(crop_img,(cx,0),(cx,720),(255,255,0),2)
  56.         cv2.line(crop_img,(0,cy),(1280,cy),(0,255,0),2)
  57.         cv2.drawContours(crop_img, contours, -1, (0,255,255), 2)
  58.  
  59.         # ---- Draw centre boundry lines (Steer straight)
  60.         cv2.line(crop_img,(270,0),(270,480),(0,0,255),2)
  61.         cv2.line(crop_img,(370,0),(370,480),(0,0,255),2)
  62.  
  63.  
  64.  # --------- Steer Right Routine ----------
  65.         if cx >= 370:
  66.             RSteer = cx - 370
  67.             SteerRight = remap(RSteer, 0, 45, 1, 270)
  68.             print ("Turn Right: ", SteerRight)
  69.  
  70.  # --------- On Track Routine ----------
  71.         if cx < 370 and cx > 270:
  72.             print "On Track"
  73.  
  74.  # --------- Steer Left Routine ----------
  75.         if cx <= 270:
  76.             LSteer = 270 - cx
  77.             SteerLeft = remap(LSteer, 0, 45, 1, 270)
  78.             print ("Turn Left: ", SteerLeft)          
  79.  
  80.     # ------ Show the resulting cropped image
  81.     cv2.imshow('frame',crop_img)
  82.     if cv2.waitKey(1) & 0xFF == ord('q'):
  83.         break
  84.  
  85.    
  86. #https://www.facebook.com/mrlunk
RAW Paste Data