diff --git a/src/main.llsp3 b/src/main.llsp3 index 5f0c27d..168abae 100644 --- a/src/main.llsp3 +++ b/src/main.llsp3 @@ -2,7 +2,6 @@ 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 @@ -21,12 +20,12 @@ import time # Productive Code ########################################################################### -async def turn_right(angle=90): +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=90): +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) @@ -38,106 +37,116 @@ async def drive_cm(cm, speed=200): await runloop.sleep_ms(cm * 70) mp.stop(mp.PAIR_1) -async def avoid_obstacle(): +async def avoid_obstacle(speed=100): 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) + 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 + old = time.ticks_ms() + speed = 100 while not won: # Farbcodes lesen - left_color = cs.color(port.C) - right_color = cs.color(port.E) - gradient = ms.tilt_angles()[2] + 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 - strait = False + straight = False + turn_way_if_lost_right = True # Distanzsensor - ds_front = ds.distance(port.F) + ds_front = ds.distance(port.D) + + if gradient < -150: + speed = 400 + + 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: + + elif ds_front < 75 and ds_front > -1: await avoid_obstacle() # Beide Sensoren sehen Grün (z.B. Kreuzung / Markierung) elif left_color == GREEN and right_color == GREEN: - mp.move_tank(mp.PAIR_1, -100, 100) + mp.move_tank(mp.PAIR_1, -speed, speed) 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) + mp.move_tank(mp.PAIR_1, speed, speed) time.sleep_ms(700) - mp.move_tank(mp.PAIR_1, -100, 100) + mp.move_tank(mp.PAIR_1, -speed, speed) time.sleep_ms(1050) - mp.move_tank(mp.PAIR_1, 100, 100) + mp.move_tank(mp.PAIR_1, speed, speed) 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) + mp.move_tank(mp.PAIR_1, speed, speed) time.sleep_ms(700) - mp.move_tank(mp.PAIR_1, 100, -100) + mp.move_tank(mp.PAIR_1, speed, -speed) time.sleep_ms(1050) - mp.move_tank(mp.PAIR_1, 100, 100) + mp.move_tank(mp.PAIR_1, speed, speed) time.sleep_ms(300) - elif right_color == BLACK and left_color == BLACK: - mp.move_tank(mp.PAIR_1, 100, 100) + elif right_color == BLACK or left_color == BLACK: + if right_color == BLACK and left_color == BLACK: + mp.move_tank(mp.PAIR_1, speed, speed) # 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) + elif left_color == BLACK and right_color == WHITE: + mp.move_tank(mp.PAIR_1, speed-100, speed) # Rechter Schwarz, linker Weiß → sanft nach rechts - elif right_color == BLACK and left_color == WHITE: - mp.move_tank(mp.PAIR_1, 50, -50) + elif right_color == BLACK and left_color == WHITE: + mp.move_tank(mp.PAIR_1, speed, speed-100) # Beide Weiß → geradeaus (vermutlich neben der Linie) elif left_color == WHITE and right_color == WHITE: - strait = True - mp.move(mp.PAIR_1, 0, velocity=150) + straight = True + mp.move_tank(mp.PAIR_1, speed, speed) + runloop.sleep_ms(50) - # TODO: ---- Weiß auf beiden Seiten = Linie verloren ---- - # wenn links == weiß UND rechts == weiß: - # langsam drehen bis schwarz gefunden + 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 - elif gradient < -150: - mp.move(mp.PAIR_1, 0, velocity=350) - - elif gradient > 50: - mp.move(mp.PAIR_1, 0, velocity=100) - - if strait: - if time.ticks_diff(time.ticks_ms(), old) > 2000: - old = time.ticks_ms - mp.move_tank(mp.PAIR_1, 100, 50) else: - old = time.ticks_ms + old = time.ticks_ms() async def main(): - mp.pair(mp.PAIR_1, port.A, port.D) + mp.pair(mp.PAIR_1, port.A, port.E) await move_on_color() - sys.exit() runloop.run(main()) \ No newline at end of file