from picamera.array import PiRGBArray
from picamera import PiCamera
import cv2import serial
import syslog
import time
import numpy as np
import RPi.GPIO as GPIO
width = 320
height = 240
tracking_width = 40
tracking_height = 40
auto_mode = 0
def t_stop():
GPIO.output(11, False)
GPIO.output(12, False)
GPIO.output(15, False)
GPIO.output(16, False)
def t_up():
GPIO.output(11, True)
GPIO.output(12, False)
GPIO.output(15, True)
GPIO.output(16, False)
time.sleep(0.05)
GPIO.output(11, False)
GPIO.output(12, False)
GPIO.output(15, False)
GPIO.output(16, False)
time.sleep(0.3)
def t_down():
GPIO.output(11, False)
GPIO.output(12, True)
GPIO.output(15, False)
GPIO.output(16, True)
def t_left():
GPIO.output(11, False)
GPIO.output(12, True)
GPIO.output(15, True)
GPIO.output(16, False)
time.sleep(0.05)
PIO.output(11, False)
GPIO.output(12, False)
GPIO.output(15, False)
GPIO.output(16, False)
time.sleep(0.3
def t_right():
GPIO.output(11, True)
GPIO.output(12, False)
GPIO.output(15, False)
GPIO.output(16, True)
time.sleep(0.05)
GPIO.output(11, False)
GPIO.output(12, False)
GPIO.output(15, False)
GPIO.output(16, False)
time.sleep(0.3)
def t_open():
GPIO.setup(22,GPIO.OUT)
GPIO.output(22,GPIO.LOW)
def t_close():
GPIO.setup(22,GPIO.IN)
def check_for_direction(position_x):
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(11,GPIO.OUT)
GPIO.setup(12,GPIO.OUT)
GPIO.setup(15,GPIO.OUT)
GPIO.setup(16,GPIO.OUT)
GPIO.setup(38,GPIO.OUT)
if position_x == 0 or position_x == width:
print 'out of bound'
t_stop()
if position_x <= ((width-tracking_width)/2 - tracking_width):
print 'move right!'
t_right()
elif position_x >= ((width-tracking_width)/2 + tracking_width):
print 'move left!'
t_left()
else:# print 'move front'
t_up()# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (width, height)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(width, height))
rawCapture2 = PiRGBArray(camera, size=(width, height))# allow the camera to warmup
time.sleep(0.1)# set the ROI (Region of Interest)
c,r,w,h = (width/2 - tracking_width/2), (height/2 - tracking_height/2), tracking_width, tracking_height
track_window = (c,r,w,h)# capture single frame of tracking image
camera.capture(rawCapture2, format='bgr')# create mask and normalized histogram
roi = rawCapture2.array[r:r+h, c:c+w]
hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_roi, np.array([0,30,32]), np.array([180,255,255]))
roi_hist = cv2.calcHist([hsv_roi], [0], mask, [180], [0,180])
cv2.normalize(roi_hist, roi_hist, 0, 255, cv2.NORM_MINMAX)
term_crit = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 80, 1)# capture frames from the camera
for frame in camera.capture_continuous(rawCapture, format='bgr', use_video_port=True):# grab the raw NumPy array representing the image, then initialize the timestamp# and occupied/unoccupied text
image = frame.array# filtering for tracking algorithm
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
dst = cv2.calcBackProject([hsv], [0], roi_hist, [0,180], 1)
ret, track_window = cv2.meanShift(dst, track_window, term_crit)
x,y,w,h = track_windowcv2.rectangle(image, (x,y), (x+w,y+h), 255, 2)
cv2.putText(image, 'Tracked', (x-25, y-10), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)# show the frame
cv2.imshow("Raspberry Pi RC Car", image)
key = cv2.waitKey(1) & 0xFF
check_for_direction(x)
time.sleep(0.01)# clear the stream in preparation for the next frame
rawCapture.truncate(0)
评论0