/
pan_tilt_tracking.py
120 lines (87 loc) · 3.19 KB
/
pan_tilt_tracking.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
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
from multiprocessing import Manager
from multiprocessing import Process
from imutils.video import WebcamVideoStream
import imutils
from servo.cobaObject import ObjCenter
from servo.pid import PID
import argparse
import signal
import time
import sys
import cv2 as cv
import servo.servoControl as sc
panServoRange = (-90, 90)
tiltServoRange = (-70, 70)
panServo = 27
tiltServo = 17
def signal_handler(sig, frame):
print("[INFO] ctrl + c pressed, exiting...")
# sc.disableServo(panServo)
# sc.disableServo(tiltServo)
sys.exit()
def obj_center(objX, objY, centerX, centerY):
signal.signal(signal.SIGINT, signal_handler)
vs = WebcamVideoStream(args.camera).start()
# time.sleep(2.0)
obj = ObjCenter()
while True:
frame = vs.read()
frame = imutils.resize(frame,width=300)
(H, W, _) = frame.shape
centerX.value = W // 2
centerY.value = H // 2
objectLoc = obj.update(frame, (centerX.value, centerY.value))
(objX.value, objY.value) = objectLoc
cv.imshow("frame", frame)
print(objectLoc)
cv.waitKey(1)
vs.stop()
def pid_process(output, p, i, d, objCoord, centerCoord):
signal.signal(signal.SIGINT, signal_handler)
p = PID(p.value, i.value, d.value)
p.initialize()
while True:
error = centerCoord.value - objCoord.value
output.value = p.update(error)
def in_range(val, start, end):
return (val >= start and val <= end)
def set_servos(pan, tlt):
signal.signal(signal.SIGINT, signal_handler)
while True:
panAngle = -1 * pan.value
tiltAngle = -1 * tlt.value
if in_range(panAngle, panServoRange[0], panServoRange[1]):
sc.setServoAngle(panServo,panAngle)
if in_range(tiltAngle, tiltServoRange[0], tiltServoRange[1]):
sc.setServoAngle(tiltServo, tiltAngle)
if __name__ == "__main__":
ap = argparse.ArgumentParser()
ap.add_argument("-c", "--camera", type=int, default=0, help="change camera")
args = ap.parse_args()
with Manager() as manager:
centerX = manager.Value("i", 0)
centerY = manager.Value("i", 0)
objX = manager.Value("i", 0)
objY = manager.Value("i", 0)
pan = manager.Value("i", 0)
tlt = manager.Value("i", 0)
panP = manager.Value("f", 0.09)
panI = manager.Value("f", 0.08)
panD = manager.Value("f", 0.002)
tiltP = manager.Value("f", 0.11)
tiltI = manager.Value("f", 0.10)
tiltD = manager.Value("f", 0.002)
processObjectCenter = Process(target=obj_center, args=(objX, objY, centerX, centerY) )
processPanning = Process(target=pid_process, args=(pan, panP, panI, panD, objX, centerX))
processTilting = Process(target=pid_process, args=(tlt, tiltP, tiltI, tiltD, objY, centerY))
processSetServos = Process(target=set_servos, args=(pan, tlt))
processObjectCenter.start()
# processPanning.start()
processTilting.start()
processSetServos.start()
processObjectCenter.join()
# processPanning.join()
processTilting.join()
processSetServos.join()
# sc.disableServo(panServo)
# sc.disableServo(tiltServo)