-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdetectcolorv2.py
105 lines (69 loc) · 2.35 KB
/
detectcolorv2.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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
import cv2
import time
import RPi.GPIO as GPIO
import json
import RGB
import Adafruit_PCA9685
import move as mv
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(50)
servoportdir = 2
vitesse=85
angle=300
temps=0.20
cap = cv2.VideoCapture(0)
cap.set(3, 640)
cap.set(4, 480)
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out = cv2.VideoWriter('color_all.avi', fourcc, 20.0, (640, 480))
def setup():
GPIO.setwarnings(False)
mv.setup()
GPIO.setmode(GPIO.BCM)
def detectColor():
ret, frame = cap.read()
frame = cv2.GaussianBlur(frame, (5, 5), 0)
hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
thresh = cv2.adaptiveThreshold(gray, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY_INV, 11, 5)
lower_red = (0, 50, 50)
upper_red = (10, 255, 255)
mask1 = cv2.inRange(hsv_frame, lower_red, upper_red)
lower_red = (170, 50, 50)
upper_red = (180, 255, 255)
mask2 = cv2.inRange(hsv_frame, lower_red, upper_red)
lower_green = (25, 52, 72)
upper_green = (102, 255, 255)
mask_green = cv2.inRange(hsv_frame, lower_green, upper_green)
mask = cv2.bitwise_or(mask1, mask2)
red_masked_frame = cv2.bitwise_and(mask, mask, mask=thresh)
green_masked_frame = cv2.bitwise_and(mask_green, mask_green, mask=thresh)
contours_red = cv2.findContours(red_masked_frame, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[1]
contours_green = cv2.findContours(green_masked_frame, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[1]
for contour_red in contours_red:
xr, yr, wr, hr = cv2.boundingRect(contour_red)
cv2.rectangle(frame, (xr, yr), (xr+wr, yr+hr), (0, 0, 255), 2)
for contour_green in contours_green:
xg, yg, wg, hg = cv2.boundingRect(contour_green)
cv2.rectangle(frame, (xg, yg), (xg+wg, yg+hg), (0, 255, 0), 2)
if cv2.countNonZero(mask) > 0:
print("R:1")
print('Stop')
mv.motorStop()
time.sleep(1)
out.write(frame)
else:
print("R:0")
print('forward')
pwm.set_pwm(servoportdir,0,angle)
mv.move(vitesse, 'forward', None)
time.sleep(temps)
out.write(frame)
setup()
while True:
detectColor()
if cv2.waitKey == ord('q'):
break
cap.release()
out.release()
cv2.destroyAllWindows()