add control GPIO

README.md

@@ -94,3 +94,6 @@ Tùy chọn ước lượng vị trí camera cùng với lưu lịch sử
 `python distance_estimation/spray_pesticide.py`
 - Thiết lập thuộc tính phù hợp với phạm vi lệch về góc, tọa độ xyz của marker so với camera, đảm bảo marker ở trung tâm, thời gian phun
 - In ra trạng thái mỗi marker
+- Khi phun, GPIO 17 được bật (kết nối với LED), phun xong sẽ tắt.
+## Thông tin các I/O trên Raspberry Pi
+![example](pinout-corrected.jpg)

distance_estimation/spray_pesticide.py

@@ -3,6 +3,9 @@ from cv2 import aruco
 import numpy as np
 import time
 import sys
+
+from gpiozero import LED
+
 import os
 sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
 from default_config import *
@@ -29,9 +32,6 @@ def get_euler_angles_from_rvec(rvec):
 def is_roll_valid(roll):
     return (abs(roll) < ALLOWED_ANGLE_DEG) or (160 <= abs(roll) <= 180)
 
-def spray_pesticide(marker_id):
-    print(f"✅ SPRAYING! Marker ID: {marker_id}")
-
 calib_data_path = f"camera_calibration/calib_data/{set_resolution}/MultiMatrix.npz"
 calib_data = np.load(calib_data_path)
 print('resolution:', set_resolution)
@@ -54,13 +54,19 @@ fps = cap.get(cv2.CAP_PROP_FPS)
 print(f"Frames per second: {fps}")
 
 
-
 def start_spraying(marker_id):
     print(f"🟡 SPRAYING marker {marker_id}...")
 
 def stop_spraying(marker_id):
     print(f"✅ SPRAYED marker {marker_id}")
 
+spray_output = LED(17)
+def update_spray_output(should_spray):
+    if should_spray:
+        spray_output.on()
+    else:
+        spray_output.off()
+
 # --- Track state ---
 marker_states = {}  # marker_id -> {"state": "NOT_SPRAYED"/"SPRAYING"/"SPRAYED", "start_time": float}
 
@@ -74,7 +80,7 @@ while True:
     marker_corners, marker_IDs, _ = detector.detectMarkers(gray_frame)
 
     current_time = time.time()
-
+    need_to_spray = False
     if marker_corners:
         rVec, tVec, _ = aruco.estimatePoseSingleMarkers(marker_corners, MARKER_SIZE, cam_mat, dist_coef)
         for i, (ids, corners) in enumerate(zip(marker_IDs, marker_corners)):
@@ -112,6 +118,10 @@ while True:
                         stop_spraying(marker_id)
                         marker_states[marker_id]["state"] = "SPRAYED"
 
+            # Shared decision: still spraying?
+            if marker_states[marker_id]["state"] in ("NOT_SPRAYED", "SPRAYING"):
+                need_to_spray = True
+
             # --- Visualization ---
             status = marker_states[marker_id]["state"]
             color = {"NOT_SPRAYED": (0, 0, 255), "SPRAYING": (0, 255, 255), "SPRAYED": (0, 255, 0)}[status]
@@ -123,6 +133,9 @@ while True:
                 cv2.putText(frame, f"{status}", (cx + 10, cy + 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
                 cv2.putText(frame, f"x:{x:.1f} y:{y:.1f} z:{z:.1f}", (cx + 10, cy + 45), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1)
                 cv2.putText(frame, f"roll:{roll:.1f} pitch:{pitch:.1f} yaw:{yaw:.1f}", (cx + 10, cy + 65), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 255), 1)
+
+    update_spray_output(need_to_spray)
+
     if visualization:
         cv2.imshow("frame", frame)
         key = cv2.waitKey(1)