detector.py

+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.")

robot_utils.py

 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"""
@@ -23,19 +24,19 @@ class RobotUtils():
 
     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)
@@ -44,12 +45,17 @@ class RobotUtils():
 
 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()
 
 
 

tictactoe.py

@@ -14,8 +14,8 @@ SYMBOLS = {
     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"