src/main.llsp3 aktualisiert

Reviewed-on: #1

README.md

@@ -1,3 +1,70 @@
 # ROB
 
-Für RoboCup 2025
+Für RoboCup 2025
+
+// ---- Ereignisse zuerst prüfen ----
+```
+    wenn links == grün UND rechts == grün:
+        // Sackgasse → 180°
+        drehe 180°
+
+    wenn links == grün ODER rechts == grün:
+        // Kreuzung → 90° Turn
+        wenn links == grün:
+            drehe 90° nach links
+        sonst:
+            drehe 90° nach rechts
+
+    // ---- Fahrbahn: Linie folgen ----
+
+    wenn links == schwarz UND rechts == weiß:
+        // Linie ist links → nach links korrigieren
+        drehe nach links
+
+    wenn rechts == schwarz UND links == weiß:
+        // Linie ist rechts → nach rechts korrigieren
+        drehe nach rechts
+
+    // ---- Geradeaus ---- 
+    wir haben im Moment wenn beide weiß und die Sensoren weiter auseinander vlt funktioniert das so smoother?
+    wenn links == schwarz UND rechts == schwarz:
+        beide Motoren gleich schnell
+
+    // ---- Weiß auf beiden Seiten = Linie verloren ----
+    wenn links == weiß UND rechts == weiß:
+        langsam drehen bis schwarz gefunden
+
+    // ---- Terrain ----
+    // 5 weil ich nicht mehr den Wert wusste
+    wenn winkel > +5:
+        Motorleistung += 20%
+        
+	 // Bergab
+    wenn winkel < -5:
+        Motorleistung -= 20%
+```
+
+### Wippe Notizen
+    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
+    
+def flipflop(state, r, s):
+    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:
+    1. Bei Drehung niedrigsten Wert des Sensors merken
+    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

src/main.llsp3

@@ -0,0 +1,169 @@
+from hub import port, motion_sensor as ms
+import motor_pair as mp
+import distance_sensor as ds
+import color_sensor as cs
+import sys
+import runloop
+import time
+
+# Implementation of Basic operations and misc. ##############################################
+
+# await mp.move_for_degrees(mp.PAIR_1, 360, 45, velocity=280)
+
+# motor.run_for_degrees(portNum, 200, 300)
+# await runloop.sleep_ms(2000)
+
+# mp.move_tank(mp.PAIR_1, 280, -280)
+# await runloop.sleep_ms(2000)
+
+# WANTED_COLOR = color.BLACK
+
+# Productive Code ###########################################################################
+
+async def turn_right(angle=85):
+    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=85):
+    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(speed=100):
+    mp.stop(mp.PAIR_1)
+    await runloop.sleep_ms(500)
+    mp.move_tank(mp.PAIR_1, -speed, -speed)
+
+    await turn_right(85)
+    await drive_cm(15)
+    await turn_left(85)
+    await drive_cm(30)
+    await turn_left(85)
+    await drive_cm(15)
+    await turn_right(85)
+old = 0
+async def move_on_color():
+    won = False
+    old = time.ticks_ms()
+    speed = 100
+    saw_green_left = False
+    saw_green_right = False
+
+    while not won:
+
+        # Farbcodes lesen
+        left_color = cs.color(port.B)
+        right_color = cs.color(port.F)
+
+        gradient = ms.tilt_angles()[2]
+        # Farbcodes
+        BLACK = 0
+        GREEN = 6
+        WHITE = 10
+        RED = 9
+        straight = False
+        turn_way_if_lost_right = True
+
+        # Distanzsensor
+        ds_front = ds.distance(port.D)
+
+        if gradient < -120:
+            speed = -80
+        elif gradient > 50:
+            speed = -50
+        else:
+            speed = -100
+
+        # --- LOGIK NUR MIT FARBEN ---
+                # Finish line logik
+        if left_color == RED or right_color == RED:
+            won = True
+            break
+
+        elif ds_front < 75 and ds_front > -1:
+            #await avoid_obstacle()
+            pass
+
+        # Linker Sensor sieht Grün → Linksabbiegen
+        elif left_color == GREEN and right_color != GREEN:
+            saw_green_left = True
+
+        elif right_color == GREEN and left_color != GREEN:
+            saw_green_right = True
+
+        # Beide Sensoren sehen Grün (z.B. Kreuzung / Markierung)
+        elif left_color == GREEN and right_color == GREEN:
+            saw_green_right = False
+            saw_green_left = False
+            mp.move_tank(mp.PAIR_1, -speed, speed)
+            time.sleep_ms(2300)
+        
+        elif saw_green_left and left_color == BLACK:
+            saw_green_left = False
+            mp.move_tank(mp.PAIR_1, speed, speed)
+            time.sleep_ms(1200)
+            mp.move_tank(mp.PAIR_1, speed, -speed)
+            time.sleep_ms(1050)
+            mp.move_tank(mp.PAIR_1, speed, speed)
+            time.sleep_ms(300)
+
+        # Rechter Sensor sieht Grün → Rechtsabbiegen
+        elif saw_green_right and right_color == BLACK:
+            saw_green_right = False
+            mp.move_tank(mp.PAIR_1, speed, speed)
+            time.sleep_ms(1200)
+            mp.move_tank(mp.PAIR_1, -speed, speed)
+            time.sleep_ms(1050)
+            mp.move_tank(mp.PAIR_1, speed, speed)
+            time.sleep_ms(300)
+
+        elif right_color == BLACK or left_color == BLACK:
+            if right_color == BLACK and left_color == BLACK:
+                mp.move_tank(mp.PAIR_1, speed, speed)
+                time.sleep_ms(20)
+
+
+        # Linker Sensor ist Schwarz, rechter Weiß → sanft nach links
+            elif left_color == BLACK and right_color == WHITE:
+                mp.move_tank(mp.PAIR_1, speed, -(int (speed / 4)))
+
+
+        # Rechter Schwarz, linker Weiß → sanft nach rechts
+            elif right_color == BLACK and left_color == WHITE:
+                mp.move_tank(mp.PAIR_1,  -(int (speed / 4)), speed)
+
+        # Beide Weiß → geradeaus (vermutlich neben der Linie)
+        elif left_color == WHITE and right_color == WHITE:
+            straight = True
+            mp.move_tank(mp.PAIR_1, speed, speed)
+            runloop.sleep_ms(50)
+
+        if straight:
+            if time.ticks_diff(time.ticks_ms(), old) > 2750:
+                old = time.ticks_ms()
+                if turn_way_if_lost_right:
+                    mp.move_tank(mp.PAIR_1, speed, 0)
+                    await runloop.sleep_ms(200)
+                    turn_way_if_lost_right = not turn_way_if_lost_right
+                else:
+                    mp.move_tank(mp.PAIR_1, 0, speed)
+                    await runloop.sleep_ms(200)
+                    turn_way_if_lost_right = not turn_way_if_lost_right
+
+        else:
+            old = time.ticks_ms()
+
+async def main():
+    mp.pair(mp.PAIR_1, port.A, port.E)
+    await move_on_color()
+    sys.exit()
+
+runloop.run(main())