Initial push

2026-02-22 23:30:15 +01:00
commit 7a2d858dfb
58 changed files with 9896 additions and 0 deletions
@@ -0,0 +1,177 @@
+#!/usr/bin/env python3
+"""
+Line following robot for Raspberry Pi 4 using AlphaBot2 and TRSensor
+Ohne LED-Visualisierung
+"""
+import time
+import RPi.GPIO as GPIO
+from AlphaBot2 import AlphaBot2
+from TRSensors import TRSensor
+
+MAX_SPEED = 25
+CORRECTION_STRENGTH = 8
+EXTRA_CURVE_CORRECTION = 2
+LINE_DETECTED_THRESHOLD = 650
+
+# Liste für gleitenden Durchschnitt
+last_outer = []
+
+def setup_gpio():
+    GPIO.setmode(GPIO.BCM)
+    GPIO.setwarnings(False)
+
+def calibrate_sensors(tr, robot):
+    print("Auto-calibrating sensors...")
+
+    for i in range(5):
+        tr.calibratedMin[i] = 1023
+        tr.calibratedMax[i] = 0
+
+    for _ in range(3):
+        robot.setPWMA(20)
+        robot.setPWMB(15)
+        robot.forward()
+        for _ in range(30):
+            raw = tr.AnalogRead()
+            for i in range(5):
+                tr.calibratedMin[i] = min(tr.calibratedMin[i], raw[i])
+                tr.calibratedMax[i] = max(tr.calibratedMax[i], raw[i])
+            time.sleep(0.02)
+
+        robot.backward()
+        for _ in range(30):
+            raw = tr.AnalogRead()
+            for i in range(5):
+                tr.calibratedMin[i] = min(tr.calibratedMin[i], raw[i])
+                tr.calibratedMax[i] = max(tr.calibratedMax[i], raw[i])
+            time.sleep(0.02)
+
+    robot.stop()
+    print("Calibration complete.")
+    print("  Min:", tr.calibratedMin)
+    print("  Max:", tr.calibratedMax)
+
+def detect_line_type(tr):
+    sensor_values = tr.readCalibrated()
+    center_avg = sum(sensor_values[1:4]) / 3
+    outer_avg = (sensor_values[0] + sensor_values[4]) / 2
+    white_line = outer_avg < center_avg
+    print("Detected line type:", "WHITE on BLACK" if white_line else "BLACK on WHITE")
+    return white_line
+
+def compute_correction(sensors):
+    left = sensors[1]
+    right = sensors[3]
+    correction = 0
+
+    if abs(left - right) < 50:
+        correction = 0
+    elif left < LINE_DETECTED_THRESHOLD and right > LINE_DETECTED_THRESHOLD:
+        correction = -CORRECTION_STRENGTH
+    elif right < LINE_DETECTED_THRESHOLD and left > LINE_DETECTED_THRESHOLD:
+        correction = CORRECTION_STRENGTH
+
+    if sensors[0] < LINE_DETECTED_THRESHOLD:
+        correction -= EXTRA_CURVE_CORRECTION
+    elif sensors[4] < LINE_DETECTED_THRESHOLD:
+        correction += EXTRA_CURVE_CORRECTION
+
+    return correction
+
+def perform_turn_if_needed(sensors, robot, position):
+    global last_outer
+    l = sensors[0]
+    r = sensors[4]
+    last_outer.append((l, r))
+    if len(last_outer) > 5:
+        last_outer.pop(0)
+
+    avg_l = sum(x for x, _ in last_outer) / len(last_outer)
+    avg_r = sum(y for _, y in last_outer) / len(last_outer)
+    delta = avg_r - avg_l
+
+    print(f"Turn-Check: AvgL={avg_l:.0f}, AvgR={avg_r:.0f}, Δ={delta:.0f}, pos={position:.1f}")
+
+    if not (1500 <= position <= 2500):
+        return False
+    if avg_l > 800 and avg_r > 800:
+        return False
+
+    if avg_r > 800 and avg_l < 500 and delta > 400:
+        print("→ 90° right-curve detected")
+        robot.stop()
+        time.sleep(0.1)
+        robot.right()
+        time.sleep(0.3)
+        robot.forward()
+        time.sleep(0.35)
+        return True
+    elif avg_l > 800 and avg_r < 500 and delta < -600 and sensors[2] < LINE_DETECTED_THRESHOLD:
+        print("← 90° left-curve detected")
+        robot.stop()
+        time.sleep(0.1)
+        robot.left()
+        time.sleep(0.3)
+        robot.forward()
+        time.sleep(0.35)
+        return True
+
+    return False
+
+def line_follow_loop(tr, robot, white_line):
+    robot.forward()
+
+    while True:
+        position, sensors = tr.readLine(white_line=white_line)
+
+        print(f"Sensors: {sensors}, pos={position:.1f}")
+
+        if perform_turn_if_needed(sensors, robot, position):
+            robot.forward()
+            time.sleep(0.35)
+            robot.stop()
+            break
+
+        if all(s > LINE_DETECTED_THRESHOLD for s in sensors):
+            time.sleep(0.05)
+            robot.backward()
+            time.sleep(0.35)
+            robot.stop()
+            continue
+
+        correction = compute_correction(sensors)
+        left_speed = min(MAX_SPEED, max(0, MAX_SPEED + correction))
+        right_speed = min(MAX_SPEED, max(0, MAX_SPEED - correction))
+
+        robot.setPWMA(int(left_speed))
+        robot.setPWMB(int(right_speed))
+
+        time.sleep(0.02)
+
+def line_follow(tr=None, robot=None):
+    if tr is None or robot is None:
+        setup_gpio()
+        robot = AlphaBot2()
+        tr = TRSensor()
+        robot.stop()
+        calibrate_sensors(tr, robot)
+
+    white_line = detect_line_type(tr)
+
+    print("Starting line follow...")
+    try:
+        line_follow_loop(tr, robot, white_line)
+    except KeyboardInterrupt:
+        print("Stopping and cleaning up...")
+        robot.stop()
+        GPIO.cleanup()
+
+if __name__ == "__main__":
+    setup_gpio()
+    robot = AlphaBot2()
+    tr = TRSensor()
+    robot.stop()
+
+    calibrate_sensors(tr, robot)
+    input("Press Enter to continue...")
+    line_follow(tr, robot)