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)