distance_sensor first implementation

src/main.llsp3

@@ -1,43 +1,12 @@
-from hub import button, port, motion_sensor as ms
-import motor
+from hub import port, motion_sensor as ms
 import motor_pair as mp
+import distance_sensor as ds
 import color_sensor as cs
 import color
 import sys
 import runloop
 import time
 
-# Notes ######################################################################################
-
-# Wippe Notizen
-def flipflop(state, r, s):
-    """
-    Logik implementiert in Rampen Logik. Bleibe stehen, wenn der Rollwinkel sich um mehr als 10 (?) ändert.
-    RS-Flipflop-Logik:
-        Set (= tue nichts), wenn der Winkel sich ändert;
-        Reset (= bleibe stehen), solange sich der Rollwinkel ändert, danach fahre normal weiter
-    """
-    return False if r else (True if s else state)
-
-# Obstacle Notizen
-"""
-- Sensor ~12cm über dem Boden einbauen; insg. 3 Abstandssensoren benötigt (oder 2 mit aufwändigerer Logik und drehendem Sensor)
- 1. Wenn vorderer Sensor bestimmten Abstand feststellt, stoppen, zurückfahren bis ein Abstand d feststeht
- 2. Drehen, bis ein Seitensensor Abstand d oder kleiner feststellt. Kleinsten Abstand bestimmen:
-    2.1 Bei Drehung niedrigsten Wert des Sensors merken
-    2.2 Wenn Abstand größer wird, zurückdrehen, bis kleinster Wert wieder erreicht ist
-3. Losfahren und in entsprechende Richtung drehen, wenn Abstand kleiner/größer wird
-4. Abbrechen, wenn Farbsensoren Schwarz erkennen
-"""
-
-# Bumper Notizen
-# Änderungen von Gier- und Neigungswinkeln, sowie das Verhalten des Roboters zu beobachten
-# Vermutlich Protokoll einbauen, was passiert, wenn für eine bestimmte Zeit die Linie nicht mehr zu finden ist.
-
-# Ziellinie Notizen
-# Wenn beide Sensoren Farbe rot feststellen, stoppe die Motoren, beende das Programm
-
-
 # Implementation of Basic operations and misc. ##############################################
 
 # await mp.move_for_degrees(mp.PAIR_1, 360, 45, velocity=280)
@@ -52,25 +21,57 @@ def flipflop(state, r, s):
 
 # Productive Code ###########################################################################
 
-async def move_on_color():
-    start_time = time.time()
-    print(ms.tilt_angles())
+async def turn_right(angle=90):
+    mp.move_tank(mp.PAIR_1, 200, -200)
+    await runloop.sleep_ms(int(angle * 7))# 6 ms per degree → tune this!
+    mp.stop(mp.PAIR_1)
 
+async def turn_left(angle=90):
+    mp.move_tank(mp.PAIR_1, -200, 200)
+    await runloop.sleep_ms(int(angle * 7))
+    mp.stop(mp.PAIR_1)
+
+# drive forward X centimeters
+async def drive_cm(cm, speed=200):
+    # convert cm → ms (you must tune this constant!)
+    mp.move_tank(mp.PAIR_1, speed, speed)
+    await runloop.sleep_ms(cm * 70)
+    mp.stop(mp.PAIR_1)
+
+async def avoid_obstacle():
+    mp.stop(mp.PAIR_1)
+    await runloop.sleep_ms(500)
+    mp.move_tank(mp.PAIR_1, -100, -100)
+
+    await turn_right(90)
+    await drive_cm(20)
+    await turn_left(90)
+    await drive_cm(40)
+    await turn_left(90)
+    await drive_cm(20)
+    await turn_right(90)
+
+async def move_on_color():
     while True:
 
         # Farbcodes lesen
         left_color = cs.color(port.C)
         right_color = cs.color(port.E)
-        gradient = ms.tilt_angles()[2]        
+        gradient = ms.tilt_angles()[2]
         # Farbcodes
         BLACK = 0
         GREEN = 6
         WHITE = 10
 
-        # --- LOGIK NUR MIT FARBEN ---
+        # Distanzsensor
+        ds_front = ds.distance(port.F)
 
+        # --- LOGIK NUR MIT FARBEN ---
         # Beide Sensoren sehen Grün (z.B. Kreuzung / Markierung)
-        if left_color == GREEN and right_color == GREEN:
+        if ds_front < 75:
+            await avoid_obstacle()
+
+        elif left_color == GREEN and right_color == GREEN:
             mp.move_tank(mp.PAIR_1, -100, 100)
             time.sleep_ms(2300)
 
@@ -92,7 +93,6 @@ async def move_on_color():
             mp.move_tank(mp.PAIR_1, 100, 100)
             time.sleep_ms(300)
 
-
         # Linker Sensor ist Schwarz, rechter Weiß → sanft nach links
         elif left_color == BLACK and right_color == WHITE:
             mp.move_tank(mp.PAIR_1, -50, 50)
@@ -107,13 +107,13 @@ async def move_on_color():
 
         # TODO: ---- Weiß auf beiden Seiten = Linie verloren ----
         # wenn links == weiß UND rechts == weiß:
-        #     langsam drehen bis schwarz gefunden
+        #    langsam drehen bis schwarz gefunden
 
         elif gradient < -150:
             mp.move(mp.PAIR_1, 0, velocity=350)
 
         elif gradient > 50:
-            mp.move(mp.PAIR_1, 0, velocity=100)  
+            mp.move(mp.PAIR_1, 0, velocity=100)   
 
 async def main():
     mp.pair(mp.PAIR_1, port.A, port.D)