New drive system

src/main.llsp3

@@ -56,53 +56,64 @@ async def move_on_color():
     start_time = time.time()
     print(ms.tilt_angles())
 
-    left = cs.reflection(port.C)
-    right = cs.reflection(port.E)
-    green = 25
-    black = 18
-    white = 90
     while True:
 
-        if left < green and left > black and right < green and right > black:
-            mp.move_tank(mp.PAIR_1, -100, 100)
-        
-        elif left < green and left > black:
-            mp.move_tank(mp.PAIR_1, -100, 100)
+        # Farbcodes lesen
+        left_color = cs.color(port.C)
+        right_color = cs.color(port.E)
+        gradient = ms.tilt_angles()[2]        
+        # Farbcodes
+        BLACK = 0
+        GREEN = 6
+        WHITE = 10
 
-        elif right < green and right > black:
+        # --- LOGIK NUR MIT FARBEN ---
+
+        # Beide Sensoren sehen Grün (z.B. Kreuzung / Markierung)
+        if left_color == GREEN and right_color == GREEN:
+            mp.move_tank(mp.PAIR_1, -100, 100)
+            time.sleep_ms(2300)
+
+        # Linker Sensor sieht Grün → Linksabbiegen
+        elif left_color == GREEN and right_color != GREEN:
+            mp.move_tank(mp.PAIR_1, 100, 100)
+            time.sleep_ms(700)
+            mp.move_tank(mp.PAIR_1, -100, 100)
+            time.sleep_ms(1200)
+            mp.move_tank(mp.PAIR_1, 100, 100)
+            time.sleep_ms(300)
+
+        # Rechter Sensor sieht Grün → Rechtsabbiegen
+        elif right_color == GREEN and left_color != GREEN:
+            mp.move_tank(mp.PAIR_1, 100, 100)
+            time.sleep_ms(700)
             mp.move_tank(mp.PAIR_1, 100, -100)
-
-        elif left < black and right > white:
-            mp.move_tank(mp.PAIR_1, -100, 100)
+            time.sleep_ms(1200)
+            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)
 
+        # Rechter Schwarz, linker Weiß → sanft nach rechts
+        elif right_color == BLACK and left_color == WHITE:
+            mp.move_tank(mp.PAIR_1, 50, -50)
 
-    # while True:
-    #     roll_value = ms.tilt_angles()[2]
-    #     # Change to switch case
-    #     if cs.reflection(port.C) < 60:
-    #         mp.move_tank(mp.PAIR_1, -100, 100)
-    #     elif cs.reflection(port.E) < 60:
-    #         mp.move_tank(mp.PAIR_1, 100, -100)
-    #     elif roll_value < -150: # uphill function
-    #         mp.move(mp.PAIR_1, 0, velocity=450)
-    #     else:
-    #         mp.move(mp.PAIR_1, 0, velocity=200)
+        # Beide Weiß → geradeaus (vermutlich neben der Linie)
+        elif left_color == WHITE and right_color == WHITE:
+            mp.move(mp.PAIR_1, 0, velocity=150)
 
-    # Geradeaus
+        # TODO: ---- Weiß auf beiden Seiten = Linie verloren ----
+        # wenn links == weiß UND rechts == weiß:
+        #     langsam drehen bis schwarz gefunden
 
-    # linkskurve
-    # rechtskurve
+        elif gradient < -150:
+            mp.move(mp.PAIR_1, 0, velocity=350)
 
-    # Kreuzung (grüner Punkt)
-    # wenn grün, dann biege um 90 Grad in die richtung ab (wenn vor schwarzer linie)
-
-    # Sackgasse (= zwei grüne Punkte)
-    # -> Drehung 180 Grad
-
-    # Bergauf
-    # Bergab
+        elif gradient > 50:
+            mp.move(mp.PAIR_1, 0, velocity=100)  
 
 async def main():
     mp.pair(mp.PAIR_1, port.A, port.D)