Compare revisions

import cv2

import numpy as np

class MarkerDetector:

    def __init__(self, camera_index=0):

        """

        Initialize the detector to find markers anywhere in the frame.

        """

        self.cap = cv2.VideoCapture(camera_index)

        # HSV color ranges

        self.green_lower = np.array([75, 50, 50])

        self.green_upper = np.array([100, 255, 255])

        self.red_lower = np.array([160,20,70])

        self.red_upper = np.array([190,255,255])

        self.min_contour_area = 400

        # Store the latest detection results

        self.marker_positions = []  # (type, (x, y))

        self.debug_frame = None

    def update(self):

        """

        Capture a new frame and detect markers anywhere.

        Returns: True if successful, False otherwise

        """

        self.marker_positions = []

        ret, frame = self.cap.read()

        if not ret:

            return False

        self.debug_frame = frame.copy()

        self._detect_markers(frame)

        return True

    def get_marker_positions(self):

        """Return list of markers as (type, (x, y)). Position is in Pixels"""

        return self.marker_positions

    def show_detection(self):

        """Display debug visualization window."""

        if self.debug_frame is not None:

            cv2.imshow("Marker Detection", self.debug_frame)

    def _detect_markers(self, frame):

        """Detect markers anywhere in the frame."""

        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        # Detect green (O) markers

        green_mask = cv2.inRange(hsv, self.green_lower, self.green_upper)

        green_contours = self._find_valid_contours(green_mask)

        for contour in green_contours:

            self._process_contour(contour, 'O')

        # Detect red (X) markers

        red_mask = cv2.inRange(hsv, self.red_lower, self.red_upper)

        red_contours = self._find_valid_contours(red_mask)

        for contour in red_contours:

            self._process_contour(contour, 'X')

    def _find_valid_contours(self, mask):

        """Find contours that meet size criteria."""

        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        valid_contours = []

        for contour in contours:

            area = cv2.contourArea(contour)

            if area > self.min_contour_area:

                valid_contours.append(contour)

        return valid_contours

    def _process_contour(self, contour, marker_type):

        """Calculate position and draw debug info for a marker contour."""

        # Calculate centroid

        M = cv2.moments(contour)

        if M["m00"] != 0:

            cX = int(M["m10"] / M["m00"])

            cY = int(M["m01"] / M["m00"])

            self.marker_positions.append((marker_type, (cX, cY)))

            # Draw debug info

            color = (0, 255, 0) if marker_type == 'O' else (0, 0, 255)

            cv2.drawContours(self.debug_frame, [contour], -1, color, 2)

            cv2.circle(self.debug_frame, (cX, cY), 7, color, -1)

if __name__ == "__main__":

    detector = MarkerDetector()

    print("Starting Marker Detection. Press 'q' to quit.")

    while True:

        success = detector.update()

        if success:

            markers = detector.get_marker_positions()

            print("\nDetected Markers:")

            for marker in markers:

                print(f"- {marker[0]} at {marker[1]}")

            detector.show_detection()

        key = cv2.waitKey(1) & 0xFF

        if key == ord('q'):

            break

    cv2.destroyAllWindows()

    print("Detection stopped.")

import phantomController as pc

import time

import traceback

PICK_HIGHT = 15

PICK_HIGHT = 10 

LIFT_HIGHT = 100

GRIP_OPEN = 511

GRIP_CLOSED = 400

GRIP_CLOSED = 380

MOVE_SPEED = 800

class RobotError(Exception):

    """Error during Robot Move"""

    def pick_place(self, start, finish) -> None:

        start.extend([LIFT_HIGHT, 0, 0, -1])

        self.move_to_pose(start, GRIP_OPEN, 1000)

        self.move_to_pose(start, GRIP_OPEN, MOVE_SPEED)

        start[2] = PICK_HIGHT

        self.move_to_pose(start, GRIP_OPEN, 1000)

        self.move_to_pose(start, GRIP_CLOSED, 1000)

        self.move_to_pose(start, GRIP_OPEN, MOVE_SPEED)

        self.move_to_pose(start, GRIP_CLOSED, MOVE_SPEED)

        start[2] = LIFT_HIGHT

        self.move_to_pose(start, GRIP_CLOSED, 1000)

        self.move_to_pose(start, GRIP_CLOSED, MOVE_SPEED)

        finish.extend([LIFT_HIGHT, 0, 0, -1])

        self.move_to_pose(finish, GRIP_CLOSED, 1000)

        self.move_to_pose(finish, GRIP_CLOSED, MOVE_SPEED)

        finish[2] = PICK_HIGHT 

        self.move_to_pose(finish, GRIP_CLOSED, 1000)

        self.move_to_pose(finish, GRIP_OPEN, 1000)

        self.move_to_pose(finish, GRIP_CLOSED, MOVE_SPEED)

        self.move_to_pose(finish, GRIP_OPEN, MOVE_SPEED)

        finish[2] = LIFT_HIGHT 

        self.move_to_pose(finish, GRIP_OPEN, 1000)

        self.move_to_pose(finish, GRIP_OPEN, MOVE_SPEED)

    def disconnect(self) -> None:

        pc.sleep(self.ser)

if __name__ == "__main__":

    robot = RobotUtils()

    try:

        robot.pick_place([130, 120], [50, 120])

        robot.disconnect()

    except:

        print(traceback.format_exc())

    while True:

        robot.pick_place([130, 0], [0, 130])

    robot.disconnect()

    #robot.move_to_pose([40, 250, 20, 0, 0, -1], 511, 1000)

    #robot.disconnect()

    #try:

    #    robot.pick_place([130, 120], [50, 120])

    #    robot.disconnect()

    #except:

    #    print(traceback.format_exc())

    #    robot.disconnect()

    Player.undefined: " "

}

COLORS = {

    Player.one: "\033[32m",  # green

    Player.two: "\033[94m",  # blue

    Player.one: "\033[91m",  # green

    Player.two: "\033[32m",  # green 

    Player.undefined: ""

}

END_COLOR = "\033[0m"