In this Video Lesson we show an initial control system that allows us to position a camera on a pan/tilt servo system to keep an object of interest in the center of the frame. The pan/tilt servo hat will continuously adjust so that the object we are tracking remains in the center of the frame. In this example we are only tracking in the ‘pan’ direction. It is left as a homework assignment for the student to extend the software to also track in the tilt direction. This should be a straightforward extension to our pan example.
|
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 106 107 |
import cv2 from picamera2 import Picamera2 import time import numpy as np from servo import Servo picam2 = Picamera2() pan=Servo(pin=13) tilt=Servo(pin=12) panAngle=0 tiltAngle=0 pan.set_angle(panAngle) tilt.set_angle(tiltAngle) dispW=1280 dispH=720 picam2.preview_configuration.main.size = (dispW,dispH) picam2.preview_configuration.main.format = "RGB888" picam2.preview_configuration.controls.FrameRate=30 picam2.preview_configuration.align() picam2.configure("preview") picam2.start() fps=0 pos=(30,60) font=cv2.FONT_HERSHEY_SIMPLEX height=1.5 weight=3 myColor=(0,0,255) def onTrack1(val): global hueLow hueLow=val print('Hue Low',hueLow) def onTrack2(val): global hueHigh hueHigh=val print('Hue High',hueHigh) def onTrack3(val): global satLow satLow=val print('Sat Low',satLow) def onTrack4(val): global satHigh satHigh=val print('Sat High',satHigh) def onTrack5(val): global valLow valLow=val print('Val Low',valLow) def onTrack6(val): global valHigh valHigh=val print('Val High',valHigh) cv2.namedWindow('myTracker') cv2.createTrackbar('Hue Low','myTracker',10,179,onTrack1) cv2.createTrackbar('Hue High','myTracker',20,179,onTrack2) cv2.createTrackbar('Sat Low','myTracker',100,255,onTrack3) cv2.createTrackbar('Sat High','myTracker',255,255,onTrack4) cv2.createTrackbar('Val Low','myTracker',100,255,onTrack5) cv2.createTrackbar('Val High','myTracker',255,255,onTrack6) while True: tStart=time.time() frame= picam2.capture_array() frame=cv2.flip(frame,-1) frameHSV=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV) cv2.putText(frame,str(int(fps))+' FPS',pos,font,height,myColor,weight) lowerBound=np.array([hueLow,satLow,valLow]) upperBound=np.array([hueHigh,satHigh,valHigh]) myMask=cv2.inRange(frameHSV,lowerBound,upperBound) myMaskSmall=cv2.resize(myMask,(int(dispW/2),int(dispH/2))) myObject=cv2.bitwise_and(frame,frame, mask=myMask) myObjectSmall=cv2.resize(myObject,(int(dispW/2),int(dispH/2))) contours,junk=cv2.findContours(myMask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) if len(contours)>0: contours=sorted(contours,key=lambda x:cv2.contourArea(x),reverse=True) #cv2.drawContours(frame,contours,-1,(255,0,0),3) contour=contours[0] x,y,w,h=cv2.boundingRect(contour) cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255),3) error=(x+w/2)-dispW/2 if error>30: panAngle=panAngle-1 if panAngle<-90: panAngle=-90 pan.set_angle(panAngle) if error<-30: panAngle=panAngle+1 if panAngle>90: panAngle=90 pan.set_angle(panAngle) cv2.imshow('Camera',frame) cv2.imshow('Mask',myMaskSmall) cv2.imshow('My Object',myObjectSmall) if cv2.waitKey(1)==ord('q'): break tEnd=time.time() loopTime=tEnd-tStart fps=.9*fps + .1*(1/loopTime) cv2.destroyAllWindows() |