In this Video Lesson we show an improved control algorithm for tracking an Object of Interest in OpenCV. We develop a simple example of Proportional control, where the correction signal is proportional to the error signal. We show this is a much improved algorithm over our earlier one, which simply applied 1 degree corrections independent of the size of the error. The code we develop in this lesson is included below for your convenience.
|
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 108 109 110 111 112 113 114 115 116 117 118 119 |
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) track=0 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) def onTrack7(val): global track track=val print('Track Value',track) 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) cv2.createTrackbar('Train-0 Track-1','myTracker',0,1,onTrack7) 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) if track==1: error=(x+w/2)-dispW/2 panAngle=panAngle-error/75 if panAngle>90: panAngle=90 if panAngle<-90: panAngle=-90 if abs(error)>35: pan.set_angle(panAngle) tiltError=(y+h/2)-dispH/2 tiltAngle=tiltAngle+tiltError/75 if tiltAngle>40: tiltAngle=40 if tiltAngle<-90: tiltAngle=-90 if abs(tiltError)>35: tilt.set_angle(tiltAngle) 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() |