OpenCV + 機械手臂
電腦視覺自動定位系統實戰

import cv2
import numpy as np

def find_circle_target(frame):
    """
    在影像中找到圓形目標，返回中心座標 (cx, cy)。
    """
    # 1. 灰階化
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 2. 高斯模糊（降噪）
    blurred = cv2.GaussianBlur(gray, (11, 11), 0)

    # 3. Canny 邊緣偵測
    edges = cv2.Canny(blurred, 50, 150)

    # 4. 找輪廓
    contours, _ = cv2.findContours(
        edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
    )

    if not contours:
        return None

    # 5. 找最大輪廓（假設目標是最顯眼的物體）
    largest = max(contours, key=cv2.contourArea)

    # 6. 計算最小外接圓
    (cx, cy), radius = cv2.minEnclosingCircle(largest)

    # 過濾太小的目標（可能是雜訊）
    if radius < 10:
        return None

    return int(cx), int(cy), int(radius)

# 測試
cap = cv2.VideoCapture(0)
while True:
    ret, frame = cap.read()
    result = find_circle_target(frame)
    if result:
        cx, cy, r = result
        cv2.circle(frame, (cx, cy), r, (0, 255, 0), 2)
        cv2.circle(frame, (cx, cy), 3, (0, 0, 255), -1)
        print(f"Target found at pixel: ({cx}, {cy})")
    cv2.imshow("Vision", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

import numpy as np
import cv2

# 校準點：[機械手臂座標(mm), 對應像素座標(px)]
# 需要手動校準一次，之後存成設定檔
CALIBRATION_POINTS = [
    # (robot_x, robot_y), (pixel_x, pixel_y)
    ((100, 100), (120, 95)),
    ((200, 100), (240, 95)),
    ((100, 200), (120, 200)),
]

def build_transform_matrix(calibration_points):
    """
    從校準點計算仿射變換矩陣。
    需要至少 3 個不共線的對應點。
    """
    robot_pts = np.float32([p[0] for p in calibration_points[:3]])
    pixel_pts = np.float32([p[1] for p in calibration_points[:3]])

    # 從像素座標到機械手臂座標的變換矩陣
    M = cv2.getAffineTransform(pixel_pts, robot_pts)
    return M

def pixel_to_robot(pixel_x, pixel_y, M):
    """
    將像素座標轉換為機械手臂座標（mm）。
    """
    pt = np.array([[[float(pixel_x), float(pixel_y)]]], dtype=np.float32)
    robot_pt = cv2.transform(pt, M)
    robot_x = robot_pt[0][0][0]
    robot_y = robot_pt[0][0][1]
    return robot_x, robot_y

# 使用方式
M = build_transform_matrix(CALIBRATION_POINTS)
target_pixel = (180, 150)
robot_x, robot_y = pixel_to_robot(*target_pixel, M)
print(f"Robot should move to: ({robot_x:.2f}mm, {robot_y:.2f}mm)")

import serial
import time

class RobotArm:
    def __init__(self, port='/dev/ttyUSB0', baudrate=115200):
        self.ser = serial.Serial(port, baudrate, timeout=1)
        time.sleep(2)  # 等待連線穩定

    def send_command(self, cmd: str):
        """發送指令並等待回應"""
        self.ser.write((cmd + '\n').encode())
        response = self.ser.readline().decode().strip()
        return response

    def move_to(self, x: float, y: float, z: float = 50.0, speed: int = 50):
        """
        移動到指定 XYZ 座標（mm）。
        z 預設 50mm 安全高度，避免撞到工件。
        """
        # 先提高 Z 軸（安全移動）
        self.send_command(f"MOVZ {z:.2f} {speed}")
        time.sleep(0.5)
        # 移動 XY
        self.send_command(f"MOVXY {x:.2f} {y:.2f} {speed}")
        time.sleep(1.0)
        return True

    def close(self):
        self.ser.close()

# 完整整合：找目標 → 轉座標 → 移動
def auto_targeting(camera_index=0):
    cap = cv2.VideoCapture(camera_index)
    robot = RobotArm('/dev/ttyUSB0')
    M = build_transform_matrix(CALIBRATION_POINTS)

    ret, frame = cap.read()
    result = find_circle_target(frame)

    if result:
        cx, cy, _ = result
        robot_x, robot_y = pixel_to_robot(cx, cy, M)
        print(f"Target at pixel ({cx}, {cy}) → robot ({robot_x:.1f}, {robot_y:.1f})")
        robot.move_to(robot_x, robot_y)
        print("✅ Movement complete")
    else:
        print("❌ Target not found")

    cap.release()
    robot.close()