Ejemplo n.º 1
0
    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:]
Ejemplo n.º 2
0
        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)
Ejemplo n.º 3
0
        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)