diff --git a/src/main.llsp3 b/src/main.llsp3 index b5989d6..f0e8f4b 100644 --- a/src/main.llsp3 +++ b/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)