128 lines
4.2 KiB
Python
128 lines
4.2 KiB
Python
|
import cv2
|
|||
|
|
import requests
|
|||
|
|
import numpy as np
|
|||
|
|
from buildhat import Motor
|
|||
|
|
import time
|
|||
|
|
from datetime import datetime
|
|||
|
|
|
|||
|
|
motorL = Motor('C')
|
|||
|
|
motorR = Motor('D')
|
|||
|
|
|
|||
|
|
# Adres MJPEG streamu z VLC
|
|||
|
|
url = "http://pilego.local:8080"
|
|||
|
|
stream = requests.get(url, stream=True)
|
|||
|
|
|
|||
|
|
# Bufor bajtów, w którym będziemy szukać klatek JPEG
|
|||
|
|
bytes_buffer = b""
|
|||
|
|
tprev = datetime.now()
|
|||
|
|
xp = 0
|
|||
|
|
ip = 0
|
|||
|
|
# Główna pętla
|
|||
|
|
for chunk in stream.iter_content(chunk_size=1024):
|
|||
|
|
bytes_buffer += chunk
|
|||
|
|
|
|||
|
|
# Szukamy pełnej klatki JPEG w bajtach
|
|||
|
|
a = bytes_buffer.find(b'\xff\xd8') # początek JPEG
|
|||
|
|
b = bytes_buffer.find(b'\xff\xd9') # koniec JPEG
|
|||
|
|
|
|||
|
|
if a != -1 and b != -1:
|
|||
|
|
# Wycinamy JPEG z bufora
|
|||
|
|
jpg = bytes_buffer[a:b + 2]
|
|||
|
|
bytes_buffer = bytes_buffer[b + 2:]
|
|||
|
|
|
|||
|
|
# Dekodujemy JPEG do obrazu (OpenCV)
|
|||
|
|
frame = cv2.imdecode(np.frombuffer(jpg, dtype=np.uint8), cv2.IMREAD_COLOR)
|
|||
|
|
if frame is None:
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
# Zmniejsz obraz (opcjonalnie) dla szybkości
|
|||
|
|
frame = cv2.resize(frame, (320, 240))
|
|||
|
|
# if frame is not None:
|
|||
|
|
# cv2.imshow("MJPEG Stream", frame)
|
|||
|
|
|
|||
|
|
# ============================
|
|||
|
|
# 👇 WYKRYWANIE NIEBIESKIEJ PIŁKI 👇
|
|||
|
|
# ============================
|
|||
|
|
|
|||
|
|
# 1. Konwersja z BGR (OpenCV) do HSV (lepszy do filtracji koloru)
|
|||
|
|
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
|
|||
|
|
|
|||
|
|
# 2. Definiujemy zakres koloru niebieskiego w HSV
|
|||
|
|
lower_blue = np.array([110, 240, 50]) # dolna granica niebieskiego
|
|||
|
|
upper_blue = np.array([130, 255, 255]) # górna granica niebieskiego
|
|||
|
|
|
|||
|
|
# 3. Maska – gdzie kolor mieści się w podanym zakresie
|
|||
|
|
mask = cv2.inRange(hsv, lower_blue, upper_blue)
|
|||
|
|
|
|||
|
|
# 4. Morfologia: usuwanie szumów (dylatacja, erozja)
|
|||
|
|
mask = cv2.erode(mask, None, iterations=2)
|
|||
|
|
mask = cv2.dilate(mask, None, iterations=2)
|
|||
|
|
|
|||
|
|
# 5. Szukamy konturów w masce
|
|||
|
|
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
|||
|
|
|
|||
|
|
# 6. Jeśli znaleziono jakiekolwiek kontury
|
|||
|
|
if contours:
|
|||
|
|
# Wybieramy największy (zakładamy, że to piłka)
|
|||
|
|
largest_contour = max(contours, key=cv2.contourArea)
|
|||
|
|
|
|||
|
|
# Jeśli kontur jest wystarczająco duży
|
|||
|
|
if cv2.contourArea(largest_contour) > 200:
|
|||
|
|
# Wyznacz środek i promień otaczającego koła
|
|||
|
|
((x, y), radius) = cv2.minEnclosingCircle(largest_contour)
|
|||
|
|
center = (int(x), int(y))
|
|||
|
|
radius = int(radius)
|
|||
|
|
|
|||
|
|
# Rysuj koło i środek na oryginalnym obrazie
|
|||
|
|
cv2.circle(frame, center, radius, (255, 0, 0), 2)
|
|||
|
|
cv2.circle(frame, center, 5, (0, 0, 255), -1)
|
|||
|
|
|
|||
|
|
# (Opcjonalnie) Wypisz pozycję
|
|||
|
|
cv2.putText(frame, f"X:{int(x)} Y:{int(y)} R:{int(radius)}", (10, 30),
|
|||
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
|||
|
|
|
|||
|
|
if int(radius) >= 100:
|
|||
|
|
motorL.stop()
|
|||
|
|
motorR.stop()
|
|||
|
|
|
|||
|
|
# if 150 < int(x) < 170:
|
|||
|
|
# motorL.stop()
|
|||
|
|
# motorR.stop()
|
|||
|
|
else:
|
|||
|
|
tnow = datetime.now()
|
|||
|
|
dtmicro = tprev - tnow
|
|||
|
|
dtms = dtmicro.seconds * 1000 + dtmicro.microseconds // 1000
|
|||
|
|
tprev = tnow
|
|||
|
|
|
|||
|
|
x = x - 160
|
|||
|
|
|
|||
|
|
wp = 0.8
|
|||
|
|
wd = 0.2
|
|||
|
|
wi = 0
|
|||
|
|
mri = 0.5
|
|||
|
|
|
|||
|
|
p = x
|
|||
|
|
d = (x - xp) / dtms
|
|||
|
|
i = mri * ip + x * dtms
|
|||
|
|
|
|||
|
|
k = wp * p + wi * i + wd * d
|
|||
|
|
|
|||
|
|
motorL.start(min(100 - k, 100))
|
|||
|
|
motorR.start(min(100 - k, 100))
|
|||
|
|
xp = x
|
|||
|
|
ip += i
|
|||
|
|
|
|||
|
|
# ============================
|
|||
|
|
# 👆 KONIEC DETEKCJI PIŁKI 👆
|
|||
|
|
# ============================
|
|||
|
|
|
|||
|
|
# Pokaż obraz z wykryciem
|
|||
|
|
if frame is not None:
|
|||
|
|
cv2.imshow("MJPEG Stream", frame)
|
|||
|
|
|
|||
|
|
# Wyjdź z pętli po wciśnięciu 'q'
|
|||
|
|
if cv2.waitKey(1) & 0xFF == ord('q'):
|
|||
|
|
break
|
|||
|
|
# Posprzątaj okna po zakończeniu
|
|||
|
|
cv2.destroyAllWindows()
|