imageCenter[1] -= 240
tailCenter[0] -= 320
tailCenter[1] -= 240
if imageCenter[0] == -320 and imageCenter[1] == -240:
imageCenter = prevImageCenter
else:
prevImageCenter = imageCenter
if tailCenter[0] == -320 and tailCenter[1] == -240:
tailCenter = prevTailCenter
else:
prevTailCenter = tailCenter
drone.angle = angleFromCenterAndTail(imageCenter, tailCenter)
d2r = lambda(deg): deg * np.pi / 180.0
r_error = -(imageCenter[0] / 500.0) * np.cos(d2r(drone.angle))
p_error = (imageCenter[1] / 500.0) * np.cos(d2r(drone.angle))
drone.update(
#roll_error=r_error,
#pitch_error=p_error,
#yaw_error=-drone.angle
)
yaw_error = np.append(yaw_error, drone.angle)[1:]
roll_error = np.append(roll_error, r_error)[1:]
target = (int(target_x), int(target_y))
center = centerFromImage(image, 10, 20)
tail = centerFromImage(image, 75, 90)
cv2.line(image, target, tuple(center), (255, 0, 0))
cv2.circle(image, tuple(center), 3, (0, 0, 255), -1)
cv2.circle(image, tuple(tail), 3, (255, 0, 0), -1)
cv2.circle(image, (int(target_x), int(target_y)), 10, (0, 0, 0), 3)
distance_x = center[0] - target[0]
distance_y = center[1] - target[1]
distance_x /= -320.0
distance_y /= -240.0
angle = angleFromCenterAndTail(center, tail) / -180.0
# logger.info("Correcting angle: %s", angle)
# if abs(angle) > 0.05:
# # logger.info("Correcting angle: %s", angle)
# distance_x = 0.0
# distance_y = 0.0
rl = x_pid.GenOut(distance_x)
fb = y_pid.GenOut(distance_y)
aa = angle_pid.GenOut(angle)
if math.isnan(aa):
aa = 0.0
drone.at_cmd(True, rl, fb, 0, aa)
target = (int(target_x), int(target_y))
center = centerFromImage(image, 10, 20)
tail = centerFromImage(image, 75, 90)
cv2.line(image, target, tuple(center), (255, 0, 0))
cv2.circle(image, tuple(center), 3, (0, 0, 255), -1)
cv2.circle(image, tuple(tail), 3, (255, 0, 0), -1)
cv2.circle(image, (int(target_x), int(target_y)), 10, (0, 0, 0), 3)
distance_x = center[0] - target[0]
distance_y = center[1] - target[1]
distance_x /= -320.0
distance_y /= -240.0
angle = angleFromCenterAndTail(center, tail) / -180.0
# logger.info("Correcting angle: %s", angle)
# if abs(angle) > 0.05:
# # logger.info("Correcting angle: %s", angle)
# distance_x = 0.0
# distance_y = 0.0
rl = x_pid.GenOut(distance_x)
fb = y_pid.GenOut(distance_y)
aa = angle_pid.GenOut(angle)
if math.isnan(aa):
aa = 0.0
drone.at_cmd(True, rl, fb, 0, aa)
