131 lines
4.2 KiB
Python
131 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)
|
||
|
||
|
||
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
|
||
|
||
v = 0.25
|
||
|
||
p = -x
|
||
d = (x - xp) * 1000 / dtms
|
||
i = mri * ip + x * dtms
|
||
|
||
k = wp * p + wi * i + wd * d
|
||
|
||
motorL.start(-v * min(100 + k, 100))
|
||
motorR.start(v * min(100 - k, 100))
|
||
xp = x
|
||
ip += i
|
||
|
||
# (Opcjonalnie) Wypisz pozycję
|
||
cv2.putText(frame, f"P:{int(p)} D:{int(d)} I:{int(i)}", (10, 30),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
|
||
|
||
# ============================
|
||
# 👆 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()
|