Initial push

robot/pid_line_follow1.py

@@ -0,0 +1,179 @@
+#!/usr/bin/env python3
+"""
+Linienfolgeroboter für Raspberry Pi 4 mit AlphaBot2 und TRSensor
+Verwendet einen PID-Regler und eine Suchfunktion bei Linienverlust.
+(Version 3: Korrektur für Stopp nach Liniensuche)
+"""
+import time
+import RPi.GPIO as GPIO
+from AlphaBot2 import AlphaBot2
+from TRSensors import TRSensor
+
+# Geschwindigkeits- und PID-Einstellungen
+MAX_SPEED = 40 # 50
+BASE_SPEED = 30 # 40
+SEARCH_SPEED = 30  # Geschwindigkeit während der Liniensuche
+
+# PID-Konstanten - DIESE MÜSSEN EVENTUELL ANGEPASST WERDEN!
+Kp = 0.045 # 0.025
+Ki = 0.0001 # 0.0005
+Kd = 0.3
+
+# Zielwert der Sensoren (Mitte der Linie)
+TARGET_POSITION = 2000
+
+# Schwelle, ab der ein Sensor die Linie als "verloren" betrachtet
+LINE_LOST_THRESHOLD = 950
+# Zeitlimit für die Suche in Sekunden
+SEARCH_TIMEOUT = 3.0
+
+IR = 17
+
+def setup_gpio():
+    """Initialisiert die GPIO-Pins."""
+    GPIO.setmode(GPIO.BCM)
+    GPIO.setwarnings(False)
+    GPIO.setup(IR, GPIO.IN)
+
+
+def calibrate_sensors(tr, robot):
+    """Führt eine automatische Kalibrierung der Sensoren durch."""
+    print("Starte Sensor-Kalibrierung...")
+    robot.setPWMA(20)
+    robot.setPWMB(20)
+    # Drehung zur Kalibrierung auf weißem und schwarzem Untergrund
+    for _ in range(2):
+        robot.right()
+        for _ in range(50):
+            tr.calibrate()
+            time.sleep(0.02)
+        robot.left()
+        for _ in range(100):
+            tr.calibrate()
+            time.sleep(0.02)
+        robot.right()
+        for _ in range(50):
+            tr.calibrate()
+            time.sleep(0.02)
+    robot.stop()
+    print("Kalibrierung abgeschlossen.")
+    print("  Min:", tr.calibratedMin)
+    print("  Max:", tr.calibratedMax)
+
+
+def search_for_line(robot, tr, last_error):
+    """
+    Sucht die Linie, indem der Roboter sich in die Richtung dreht,
+    in der die Linie zuletzt war.
+    """
+    print("Linie verloren! Suche...")
+    robot.stop()
+    time.sleep(0.1)
+
+    # Setze zuerst die Geschwindigkeit für beide Motoren
+    robot.setPWMA(SEARCH_SPEED)
+    robot.setPWMB(SEARCH_SPEED)
+
+    # Bestimme dann die Suchrichtung
+    if last_error > 0:
+        print("Suche nach links...")
+        robot.left()
+    else:
+        print("Suche nach rechts...")
+        robot.right()
+
+    start_time = time.time()
+    while time.time() - start_time < SEARCH_TIMEOUT:
+        # Lies kontinuierlich die Sensoren, während du dich drehst
+        _, sensors = tr.readLine(white_line=False)
+
+        # Prüfe, ob einer der Sensoren die Linie wieder sieht
+        if any(s < LINE_LOST_THRESHOLD for s in sensors):
+            print("Linie wiedergefunden!")
+            robot.stop()
+            time.sleep(0.1)
+            return True  # Suche war erfolgreich
+
+        time.sleep(0.01)
+
+    # Schleife beendet, ohne die Linie zu finden (Timeout)
+    print(f"Suche nach {SEARCH_TIMEOUT}s erfolglos. Roboter stoppt.")
+    robot.stop()
+    return False  # Suche war erfolglos
+
+
+def line_follow_loop(tr, robot):
+    """Hauptschleife für die Linienverfolgung mittels PID-Regelung."""
+    # Setzt die anfängliche Richtung auf vorwärts
+    robot.forward()
+
+    last_error = 0
+    integral = 0
+
+    while True:
+        # Lese die Position der Linie. False für schwarze Linie auf weißem Grund.
+        position, sensors = tr.readLine(white_line=False)
+
+        # Wenn alle Sensoren weiß sehen, starte die Liniensuche
+        if all(s > LINE_LOST_THRESHOLD for s in sensors):
+            search_successful = search_for_line(robot, tr, last_error)
+            if search_successful:
+                # Setze PID-Werte zurück, um einen Sprung zu vermeiden
+                last_error = 0
+                integral = 0
+                # WICHTIG: Setze die Richtung wieder auf vorwärts, bevor die Schleife weitergeht
+                robot.forward()
+                continue
+            else:
+                # Beende die Hauptschleife, wenn die Suche fehlschlägt
+                break
+
+                # --- PID-Berechnung ---
+        error = position - TARGET_POSITION
+        integral += error
+        derivative = error - last_error
+        last_error = error
+
+        correction = (Kp * error) + (Ki * integral) + (Kd * derivative)
+
+        # Anpassung der Motor-Geschwindigkeiten
+        left_speed = BASE_SPEED + correction
+        right_speed = BASE_SPEED - correction
+
+        # Geschwindigkeiten auf den gültigen Bereich begrenzen
+        left_speed = max(0, min(MAX_SPEED, left_speed))
+        right_speed = max(0, min(MAX_SPEED, right_speed))
+
+        # print(f"Pos: {position:.0f} | Err: {error:.0f} | Corr: {correction:.2f} | L: {left_speed:.0f} | R: {right_speed:.0f}")
+
+        robot.setPWMA(int(left_speed))
+        robot.setPWMB(int(right_speed))
+
+        time.sleep(0.01)
+
+
+def follow(tr , robot):
+    """Hauptfunktion des Programms."""
+    #robot = AlphaBot2()
+    #tr = TRSensor()
+    #robot.stop()
+
+    try:
+        calibrate_sensors(tr, robot)
+        print("Kalibrierung abgeschlossen")
+        time.sleep(5.0)
+        #input("Kalibrierung abgeschlossen. Drücke Enter zum Starten...")
+        line_follow_loop(tr, robot)
+    except KeyboardInterrupt:
+        print("\nProgramm gestoppt.")
+    finally:
+        robot.stop()
+
+if __name__ == "__main__":
+    setup_gpio()
+    robot = AlphaBot2()
+    tr = TRSensor()
+    robot.stop()
+    follow(tr, robot)
+    print("Räume GPIO auf.")
+    GPIO.cleanup()