Untitled

import cv2
import numpy as np
import RPi.GPIO as GPIO
import time

# GPIO-Setup
GPIO.setmode(GPIO.BCM)
RED_PIN = 17
GREEN_PIN = 27
BLUE_PIN = 22

GPIO.setup(RED_PIN, GPIO.OUT)
GPIO.setup(GREEN_PIN, GPIO.OUT)
GPIO.setup(BLUE_PIN, GPIO.OUT)

# Funktion zum Setzen der GPIO-Ausgaenge (LED-Farben)
def set_color_output(color):
    GPIO.output(RED_PIN, GPIO.HIGH if color == 'red' else GPIO.LOW)
    GPIO.output(GREEN_PIN, GPIO.HIGH if color == 'green' else GPIO.LOW)
    GPIO.output(BLUE_PIN, GPIO.HIGH if color == 'blue' else GPIO.LOW)

# GStreamer-Pipeline fuer die Kamera
gstreamer_pipeline = (
    "libcamerasrc ! "
    "video/x-raw,format=BGR ! "
    "videoconvert ! "
    "appsink"
)
camera = cv2.VideoCapture(gstreamer_pipeline, cv2.CAP_GSTREAMER)
if not camera.isOpened():
    print("Fehler beim Zugriff auf die Kamera ueber die GStreamer-Pipeline")
    exit()

# Funktion zur Farberkennung
def detect_color(frame):
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    # Farbgrenzen definieren
    color_ranges = {
        'red': ((0, 120, 70), (10, 255, 255)),  # Rot
        'green': ((36, 50, 70), (89, 255, 255)),  # Gruen
        'blue': ((90, 50, 70), (130, 255, 255))  # Blau
    }

    detected_color = None

    # Ueberpruefe jede Farbe
    for color, (lower, upper) in color_ranges.items():
        lower = np.array(lower, dtype=np.uint8)
        upper = np.array(upper, dtype=np.uint8)
        mask = cv2.inRange(hsv, lower, upper)
        if np.any(mask):
            detected_color = color
            break

    return detected_color

# Funktion zur Kreiserkennung
def detect_circle(frame):
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # Kreise erkennen
    circles = cv2.HoughCircles(
        gray, cv2.HOUGH_GRADIENT, dp=1.2, minDist=50,
        param1=100, param2=30, minRadius=30, maxRadius=60  # Angepasste min/max Radiuswerte
    )

    if circles is not None:
        circles = np.round(circles[0, :]).astype("int")
        pixel_to_mm_ratio = 0.179  # Kalibrierte Pixel-zu-mm Umrechnung

        for circle in circles:
            x, y, radius = circle
            diameter_pixels = 2 * radius  # Durchmesser in Pixeln
            diameter_mm = diameter_pixels * pixel_to_mm_ratio  # Durchmesser in mm

            # Ausgabe der Kreisdimensionen
            #print(f"Erkannter Kreis: Durchmesser in Pixeln = {diameter_pixels}, Durchmesser in mm = {diameter_mm:.2f}mm")

            # Überprüfen, ob der Durchmesser im erwarteten Bereich liegt (z.B. 5 cm Kreis)
            if 20 <= diameter_mm <= 50:
                return True, radius, diameter_pixels, diameter_mm
            else:
                return False, radius, diameter_pixels, diameter_mm

    return False, None, None, None

try:
    check_shape = True  # Auf False setzen, um nur die Farbe zu überprüfen
    while True:
        ret, frame = camera.read()
        if not ret:
            print("Fehler beim Lesen des Kamerabilds")
            break

        # Farbe erkennen
        detected_color = detect_color(frame)
        is_circle, radius, diameter_pixels, diameter_mm = detect_circle(frame) if check_shape else (True, None, None, None)

        # Falls ein Kreis erkannt wird, Dimensionen in Pixeln und mm ausgeben
        if is_circle and diameter_pixels:
            print(f"Kreisdimensionen: Durchmesser = {diameter_pixels} Pixel, Radius = {radius} Pixel, Durchmesser = {diameter_mm:.2f} mm")

        if detected_color:
            if check_shape:
                if is_circle:
                    print(f"Erkannte Farbe: {detected_color} - Kreis erkannt - Durchmesser >>> {diameter_mm:.2f}mm")
                elif diameter_pixels:
                    print(f"Erkannte Farbe: {detected_color} - Kreis erkannt, aber falsche Groesse: {diameter_mm:.2f} mm")
                else:
                    print(f"Erkannte Farbe: {detected_color} - Kein gueltiger Kreis gefunden")
            else:
                print(f"Erkannte Farbe: {detected_color}")
            set_color_output(detected_color)
        else:
            if is_circle:
                print("Farbe: Nicht definiert - Kreisform erkannt")
            elif diameter_pixels:
                print(f"Farbe: Nicht definiert - Kreis erkannt, aber falsche Groesse: {diameter_mm:.2f} mm")
            else:
                print("Farbe: Nicht definiert")
            set_color_output(None)

        # Pausiere, um den Prozessor nicht zu überlasten
        time.sleep(2)
        
except KeyboardInterrupt:
    print("Programm unterbrochen")

finally:
    # Aufräumen
    camera.release()
    GPIO.cleanup()