-
Notifications
You must be signed in to change notification settings - Fork 0
/
algo.py
89 lines (65 loc) · 2.33 KB
/
algo.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
import numpy as np
import cv2
#import Adafruit_BBIO.GPIO as GPIO
#video_capture = cv2.VideoCapture(-1)
#video_capture.set(3, 160)
#video_capture.set(4, 120)
# Setup Output Pins
#Left Forward
#GPIO.setup("P8_10", GPIO.OUT)
#Right Forward
#GPIO.setup("P9_11", GPIO.OUT)
#GPIO.output("P8_10", GPIO.HIGH)
#GPIO.output("P9_11", GPIO.HIGH)
def follow(image):
if(autonomousMode):
# Capture the frames
#ret, frame = video_capture.read()
frame = image;
# Crop the image
crop_img = frame[60:120, 0:160]
# Convert to grayscale
gray = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV)
# Gaussian blur
blur = cv2.GaussianBlur(gray,(5,5),0)
# Color thresholding
ret,thresh1 = cv2.threshold(blur,60,255,cv2.THRESH_BINARY_INV)
# Erode and dilate to remove accidental line detections
mask = cv2.erode(thresh1, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# Find the contours of the frame
contours,hierarchy = cv2.findContours(mask.copy(), 1, cv2.CHAIN_APPROX_NONE)
# Find the biggest contour (if detected)
if len(contours) > 0:
c = max(contours, key=cv2.contourArea)
M = cv2.moments(c)
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
cv2.line(crop_img,(cx,0),(cx,600),(255,0,0),1)
cv2.line(crop_img,(0,cy),(800,cy),(255,0,0),1)
cv2.drawContours(crop_img, contours, -1, (0,255,0), 1)
print cx
print cy
#if cx >= 140:
#GPIO.output("P8_10", GPIO.HIGH)
#GPIO.output("P9_11", GPIO.LOW)
#controller.move_left()
#controller.move_up()
#else if cx < 140 and cx > 30:
#GPIO.output("P8_10", GPIO.LOW)
#GPIO.output("P9_11", GPIO.LOW)
#controller.stop()
#else if cx <= 30:
#controller.move_right()
#controller.move_up()
#else:
#controller.stop()
#else:
#controller.move_up()
#Display the resulting frame
cv2.imshow('frame',crop_img)
#if(cy > 80):
#controller.move_forward()
#if(cx > )
if cv2.waitKey(1) & 0xFF == ord('q'):
break