Ball Balancing Robot(PyCharm)

La issue contiene uno script Python autonomo, ma non indica alcun file del repository, test o modifica richiesta. Inizia chiarendo se è destinato a essere aggiunto al progetto, quindi esamina i punti di ingresso della fotocamera e del rilevamento ArUco di OpenCV insieme alla connessione seriale ad Arduino. Nell’issue non sono definiti criteri di completamento.

import cv2
import numpy as np
import serial
import time
import math

=== CONFIG ===

SERIAL_PORT = 'COM3'
BAUD_RATE = 9600
FRAME_WIDTH, FRAME_HEIGHT = 640, 480
MAX_ANGLE_CHANGE = 25

=== COLOR RANGE ===

lower_ball = np.array([10, 100, 70]) # Orange ball
upper_ball = np.array([25, 255, 255])

lower_disc = np.array([35, 40, 40]) # Green disc
upper_disc = np.array([85, 255, 255])

=== ARUCO ===

aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
aruco_params = cv2.aruco.DetectorParameters()

=== SERVO CENTER ANGLES ===

CENTER_ANGLES = [60, 75, 60]

=== PD CONSTANTS ===

Kp = 0.4
Kd = 0.20

PD state

prev_dx, prev_dy = 0, 0
prev_time = time.time()

Connect to Arduino

arduino = serial.Serial(SERIAL_PORT, BAUD_RATE)
time.sleep(2)

Start webcam

cap = cv2.VideoCapture(2)

def get_servo_angles(dx, dy, ddx, ddy):
dx /= (FRAME_WIDTH / 2)
dy /= (FRAME_HEIGHT / 2)
ddx /= (FRAME_WIDTH / 2)
ddy /= (FRAME_HEIGHT / 2)

# Apply PD control
control_x = Kp * dx + Kd * ddx
control_y = Kp * dy + Kd * ddy

angle1 = CENTER_ANGLES[0] + (-control_x * math.cos(math.radians(0)) - control_y * math.sin(math.radians(0))) * MAX_ANGLE_CHANGE
angle2 = CENTER_ANGLES[1] + (-control_x * math.cos(math.radians(120)) - control_y * math.sin(math.radians(120))) * MAX_ANGLE_CHANGE
angle3 = CENTER_ANGLES[2] + (-control_x * math.cos(math.radians(240)) - control_y * math.sin(math.radians(240))) * MAX_ANGLE_CHANGE

return int(angle1), int(angle2), int(angle3)

while True:
ret, frame = cap.read()
if not ret:
break

frame = cv2.resize(frame, (FRAME_WIDTH, FRAME_HEIGHT))
frame = cv2.flip(frame, 1)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

# ===== 1. Detect ArUco Marker =====
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=aruco_params)

center_x, center_y = FRAME_WIDTH // 2, FRAME_HEIGHT // 2  # fallback

if corners:
    c = corners[0][0]
    center_x = int(np.mean(c[:, 0]))
    center_y = int(np.mean(c[:, 1]))
    cv2.polylines(frame, [c.astype(int)], True, (255, 0, 255), 2)
    cv2.circle(frame, (center_x, center_y), 5, (255, 0, 255), -1)
    cv2.putText(frame, "Aruco", (center_x + 10, center_y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 255), 1)

# ===== 2. Disc Detection =====
disc_mask = cv2.inRange(hsv, lower_disc, upper_disc)
disc_mask = cv2.morphologyEx(disc_mask, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8))

disc_contours, _ = cv2.findContours(disc_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
valid_area_mask = np.zeros_like(disc_mask)

if disc_contours:
    largest_disc = max(disc_contours, key=cv2.contourArea)
    cv2.drawContours(frame, [largest_disc], -1, (0, 255, 255), 2)
    cv2.drawContours(valid_area_mask, [largest_disc], -1, 255, -1)

# ===== 3. Ball Detection =====
ball_mask = cv2.inRange(hsv, lower_ball, upper_ball)
ball_mask = cv2.bitwise_and(ball_mask, ball_mask, mask=valid_area_mask)
ball_mask = cv2.erode(ball_mask, None, iterations=2)
ball_mask = cv2.dilate(ball_mask, None, iterations=2)

ball_contours, _ = cv2.findContours(ball_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

if ball_contours:
    c = max(ball_contours, key=cv2.contourArea)
    ((x, y), radius) = cv2.minEnclosingCircle(c)

    if radius > 8:
        cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 0), 2)
        dx = x - center_x
        dy = y - center_y

        # PD derivative calculation
        current_time = time.time()
        dt = current_time - prev_time if current_time != prev_time else 1e-3
        ddx = (dx - prev_dx) / dt
        ddy = (dy - prev_dy) / dt

        # Get new servo angles
        angle1, angle2, angle3 = get_servo_angles(dx, dy, ddx, ddy)
        print(f"Ball: ({int(x)}, {int(y)}) Δx: {int(dx)} Δy: {int(dy)} | Angles: {angle1}, {angle2}, {angle3}")
        arduino.write(f"{angle1},{angle2},{angle3}\n".encode())

        # Update previous for next frame
        prev_dx, prev_dy = dx, dy
        prev_time = current_time

cv2.imshow("Balancing Bot", frame)
if cv2.waitKey(1) & 0xFF == 27:
    break

cap.release()
arduino.close()
cv2.destroyAllWindows()