def action(img_bytes, img_width, img_height):
# Set up command defaults
global pix_sum_old
global pix_sum_0
global idx
vx = 0
vy = 0
vz = 0
wx = 0
wy = 0
wz = 0
#首次运行时的初始化设置
if idx == 0:
ctr, pix_sum_0 = color_tracker.track(img_bytes)
pix_sum_0 = 1000
idx += 1
pix_sum_old = pix_sum_0
# Set up state variables first time around
if not hasattr(action, 'count'):
action.count = 0
action.errx_1 = 0
action.erry_1 = 0
action.phi_1 = 0
action.gaz_1 = 0
# Grab centroid of green ball
ctr, pix_sum = color_tracker.track(img_bytes)
if ctr == (-1, -1):
return (0, 0, 0, 0, 0, 0)
# Use centroid if it exists
if pix_sum / float(pix_sum_0) > 0:
# Compute proportional distance (error) of centroid from image center
erry = _dst(ctr, 0, img_width)
errx = 1 - pix_sum / float(pix_sum_0)
pix_sum_old = pix_sum
# Compute vertical, horizontal velocity commands based on PID control after first iteration
if action.count > 0:
vx = _pid(errx, action.errx_1, Kpx, Kix, Kdx)
vy = -_pid(erry, action.erry_1, Kpy, Kiy, Kdy)
# Remember PID variables for next iteration
action.errx_1 = errx
action.erry_1 = erry
action.count += 1
# Send control parameters back to drone
return (vx, vy, vz, wx, wy, wz)
def action(img_bytes, img_width, img_height):
# Set up command defaults
global pix_sum_old
global pix_sum_0
global idx
vx = 0
vy = 0
vz = 0
wx = 0
wy = 0
wz = 0
#首次运行时的初始化设置
if idx == 0:
ctr,pix_sum_0 = color_tracker.track(img_bytes)
pix_sum_0 = 1000
idx += 1
pix_sum_old = pix_sum_0
# Set up state variables first time around
if not hasattr(action, 'count'):
action.count = 0
action.errx_1 = 0
action.erry_1 = 0
action.phi_1 = 0
action.gaz_1 = 0
# Grab centroid of green ball
ctr,pix_sum = color_tracker.track(img_bytes)
if ctr == (-1,-1):
return (0,0,0,0,0,0)
# Use centroid if it exists
if pix_sum / float(pix_sum_0) > 0:
# Compute proportional distance (error) of centroid from image center
erry = _dst(ctr, 0, img_width)
errx = 1 - pix_sum/float(pix_sum_0)
pix_sum_old = pix_sum
# Compute vertical, horizontal velocity commands based on PID control after first iteration
if action.count > 0:
vx = _pid( errx, action.errx_1, Kpx, Kix, Kdx)
vy = -_pid( erry, action.erry_1, Kpy, Kiy, Kdy)
# Remember PID variables for next iteration
action.errx_1 = errx
action.erry_1 = erry
action.count += 1
# Send control parameters back to drone
return (vx,vy,vz,wx,wy,wz)
# Returns proportional distance to image center along specified dimension.
# Above center = -; Below = +
# Right of center = +; Left = 1
def _dst(ctr, dim, siz):
siz = siz / 2
return (ctr[dim] - siz) / float(siz)
cv2.imshow(WINDOW_NAME, image)
# Force image display, setting centroid to None on ESC key input
if cv2.waitKey(1) & 0xFF == 27:
ctr = None
# Return coordinates of centroid
return ctr
# Test with input from camera
if __name__ == '__main__':
capture = cv2.VideoCapture(0)
while True:
okay, image = capture.read()
if okay:
if not color_tracker.track(image):
break
if cv2.waitKey(1) & 0xFF == 27:
break
else:
print('Capture failed')
break
return (vx,vy,vz,wx,wy,wz)
# Simple PID controller from http://www.control.com/thread/1026159301
def _pid(err, err_1, Kp, Ki, Kd):
return Kp*err + Ki*(err+err_1) + Kd*(err-err_1)
# Returns proportional distance to image center along specified dimension.
# Above center = -; Below = +
# Right of center = +; Left = 1
def _dst(ctr, dim, siz):
siz = siz/2
return (ctr[dim] - siz) / float(siz)
cv2.imshow(WINDOW_NAME, image)
# Force image display, setting centroid to None on ESC key input
if cv2.waitKey(1) & 0xFF == 27:
ctr = None
# Return coordinates of centroid
return ctr
# Test with input from camera
if __name__ == '__main__':
capture = cv2.VideoCapture(0)
while True:
okay, image = capture.read()
if okay:
if not color_tracker.track(image):
break
if cv2.waitKey(1) & 0xFF == 27:
break
else:
print('Capture failed')
break