pid steering

cam-sample.py

@@ -0,0 +1,47 @@
+import cv2
+import time
+
+# Initialize video capture
+capture = cv2.VideoCapture(0)
+capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1024)
+capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 576)
+capture.set(cv2.CAP_PROP_FPS, 30)  # Requesting 30 FPS from the camera
+
+# Background subtractor
+bg_subtractor = cv2.createBackgroundSubtractorMOG2()
+
+# Current mode
+mode = "normal"
+
+# FPS calculation
+prev_time = time.time()
+
+# Video writer initialization with explicit FPS
+save_fps = 15.0  # Adjust this based on your actual processing speed
+fourcc = cv2.VideoWriter_fourcc(*"XVID")
+out = cv2.VideoWriter("output.avi", fourcc, save_fps, (1024, 576))
+
+# Sprawdź, czy kamera została poprawnie otwarta
+if not capture.isOpened():
+    print("Nie można otworzyć kamery!")
+    exit()
+
+while True:
+    # Przeczytaj jedną klatkę z kamery
+    ret, frame = capture.read()
+
+    # Jeśli nie udało się odczytać klatki - przerwij
+    if not ret:
+        print("Nie udało się odczytać klatki!")
+        break
+
+    # Pokaż klatkę w oknie
+    cv2.imshow('Podgląd z kamery', frame)
+
+    # Przerwij pętlę po naciśnięciu klawisza 'q'
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
+
+# Zwolnij zasoby
+capture.release()
+cv2.destroyAllWindows()

detection-sample.py

@@ -0,0 +1,90 @@
+import cv2
+import requests
+import numpy as np
+
+# 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, 210, 10])    # 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) > 1:
+                # 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)
+
+        # ============================
+        # 👆 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()

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()

picamera2-sample.py

@@ -0,0 +1,14 @@
+from picamera2 import Picamera2
+import cv2
+
+picam2 = Picamera2()
+picam2.configure(picam2.create_preview_configuration(main={"size": (1024, 576)}))
+picam2.start()
+
+while True:
+    frame = picam2.capture_array()
+    cv2.imshow("Podgląd", frame)
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
+
+cv2.destroyAllWindows()

sample-server.py

@@ -0,0 +1,27 @@
+import cv2
+
+# Adres strumienia MJPEG z VLC
+stream_url = "http://pilego.local:8080"
+
+# Otwórz strumień jako źródło wideo
+cap = cv2.VideoCapture(stream_url)
+cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+
+if not cap.isOpened():
+    print("Nie można otworzyć kamery!")
+    exit()
+
+while True:
+    ret, frame = cap.read()
+    if not ret:
+        print("Nie udało się odczytać klatki!")
+        break
+
+    cv2.imshow("Podgląd z kamery", frame)
+
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
+
+cap.release()
+cv2.destroyAllWindows()

server-stream.py

@@ -0,0 +1,25 @@
+import cv2
+import requests
+import numpy as np
+
+url = "http://pilego.local:8080"
+stream = requests.get(url, stream=True)
+
+bytes_buffer = b""
+
+for chunk in stream.iter_content(chunk_size=1024):
+    bytes_buffer += chunk
+    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:
+        jpg = bytes_buffer[a:b+2]
+        bytes_buffer = bytes_buffer[b+2:]
+
+        img = cv2.imdecode(np.frombuffer(jpg, dtype=np.uint8), cv2.IMREAD_COLOR)
+        if img is not None:
+            cv2.imshow("MJPEG Stream", img)
+
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            break
+
+cv2.destroyAllWindows()

steer-pid.py

@@ -0,0 +1,134 @@
+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, 210, 1])  # 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) > 1:
+                # 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 = tnow - tprev
+                    dtms = dtmicro.seconds * 1000 + dtmicro.microseconds // 1000
+                    tprev = tnow
+
+                    x = x - 160
+
+                    wp = 0.6
+                    wd = 0.35
+                    wi = 0.05
+                    mri = 0.2
+
+                    v = 0.25
+
+                    p = -x
+                    d = -(radius/100) * (x - xp) * 1000 / dtms
+                    i = -mri * ip + x * dtms / 10000
+
+                    k = wp * p + wi * i + wd * d
+
+                    speedL = -1 * max(-99, v * min(100 + k, 99))
+                    speedR = max(-99, v * min(100 - k, 99))
+                    print(p, d, i, speedL, speedR)
+                    motorL.start(int(speedL))
+                    motorR.start(int(speedR))
+                    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)
+                    #print("PDI: ", p, d, dtms, i)
+            else:
+                ip = 0
+        # ============================
+        # 👆 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()

super-simple.py

@@ -0,0 +1,21 @@
+import cv2
+
+cap = cv2.VideoCapture(0)
+
+if not cap.isOpened():
+    print("Nie można otworzyć kamery!")
+    exit()
+
+while True:
+    ret, frame = cap.read()
+    if not ret:
+        print("Nie udało się odczytać klatki!")
+        break
+
+    cv2.imshow("Podgląd z kamery", frame)
+
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
+
+cap.release()
+cv2.destroyAllWindows()

test.py

@@ -0,0 +1,18 @@
+import time
+from datetime import datetime
+
+# t1 = datetime.now()
+# time.sleep(1.345345)
+# t2 = datetime.now()
+# dtmicro=t2-t1
+# dtms = dtmicro.seconds * 1000 + dtmicro.microseconds//1000
+
+tprev = datetime.now()
+
+# while True:
+tnow = datetime.now()
+dtmicro = tnow - tprev
+# dtms = dtmicro.seconds * 1000 + dtmicro.microseconds // 1000
+dtms = dtmicro.microseconds
+print(tnow, tprev, dtms)
+tprev = tnow