diff --git a/detection-steer.py b/detection-steer.py new file mode 100644 index 0000000..66c6784 --- /dev/null +++ b/detection-steer.py @@ -0,0 +1,104 @@ +import cv2 +import requests +import numpy as np +from buildhat import Motor +import time + +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"" + +# 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: + motorL.start(int((min(100, 260 - int(x)))/4)) + motorR.start(int((min(100, int(x) - 60))/4)) + + # ============================ + # 👆 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()