From 0527e81de96fa6f681e94348f0d1c87d85e0cc9e Mon Sep 17 00:00:00 2001
From: tymek <tymek@yx.dom>
Date: Thu, 31 Jul 2025 18:20:14 +0200
Subject: [PATCH] pid steering

---
 steer-pid.py | 127 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 127 insertions(+)
 create mode 100644 steer-pid.py

diff --git a/steer-pid.py b/steer-pid.py
new file mode 100644
index 0000000..2238045
--- /dev/null
+++ b/steer-pid.py
@@ -0,0 +1,127 @@
+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()