2211567
/
ROB
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Distance sensor bug fix; speed variable implementation

master
Cedric Hermann 2025-11-27 11:15:26 +01:00
parent b75bcda25a
commit 6560ce3cac
1 changed files with 61 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
src/main.llsp3
View File

@ -2,7 +2,6 @@ from hub import port, motion_sensor as ms
import motor_pair as mp import motor_pair as mp
import distance_sensor as ds import distance_sensor as ds
import color_sensor as cs import color_sensor as cs
import color
import sys import sys
import runloop import runloop
import time import time
@ -21,12 +20,12 @@ import time
# Productive Code ########################################################################### # Productive Code ###########################################################################
async def turn_right(angle=90): async def turn_right(angle=85):
mp.move_tank(mp.PAIR_1, 200, -200) mp.move_tank(mp.PAIR_1, 200, -200)
await runloop.sleep_ms(int(angle * 7))# 6 ms per degree → tune this! await runloop.sleep_ms(int(angle * 7))# 6 ms per degree → tune this!
mp.stop(mp.PAIR_1) 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) mp.move_tank(mp.PAIR_1, -200, 200)
await runloop.sleep_ms(int(angle * 7)) await runloop.sleep_ms(int(angle * 7))
mp.stop(mp.PAIR_1) mp.stop(mp.PAIR_1)
@ -38,106 +37,116 @@ async def drive_cm(cm, speed=200):
await runloop.sleep_ms(cm * 70) await runloop.sleep_ms(cm * 70)
mp.stop(mp.PAIR_1) mp.stop(mp.PAIR_1)
async def avoid_obstacle(): async def avoid_obstacle(speed=100):
mp.stop(mp.PAIR_1) mp.stop(mp.PAIR_1)
await runloop.sleep_ms(500) await runloop.sleep_ms(500)
mp.move_tank(mp.PAIR_1, -100, -100) mp.move_tank(mp.PAIR_1, -speed, -speed)
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)
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(): async def move_on_color():
won = False won = False
old = time.ticks_ms old = time.ticks_ms()
speed = 100
while not won: while not won:
# Farbcodes lesen # Farbcodes lesen
left_color = cs.color(port.C) left_color = cs.color(port.B)
right_color = cs.color(port.E) right_color = cs.color(port.F)
gradient = ms.tilt_angles()[2]
gradient = ms.tilt_angles()[2]
# Farbcodes # Farbcodes
BLACK = 0 BLACK = 0
GREEN = 6 GREEN = 6
WHITE = 10 WHITE = 10
RED = 9 RED = 9
strait = False straight = False
turn_way_if_lost_right = True
# Distanzsensor # 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 --- # --- LOGIK NUR MIT FARBEN ---
# Finish line logik # Finish line logik
if left_color == RED or right_color == RED: if left_color == RED or right_color == RED:
won = True won = True
break break
elif ds_front < 75: elif ds_front < 75 and ds_front > -1:
await avoid_obstacle() await avoid_obstacle()
# Beide Sensoren sehen Grün (z.B. Kreuzung / Markierung) # Beide Sensoren sehen Grün (z.B. Kreuzung / Markierung)
elif left_color == GREEN and right_color == GREEN: 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) time.sleep_ms(2300)
# Linker Sensor sieht Grün → Linksabbiegen # Linker Sensor sieht Grün → Linksabbiegen
elif left_color == GREEN and right_color != GREEN: 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) time.sleep_ms(700)
mp.move_tank(mp.PAIR_1, -100, 100) mp.move_tank(mp.PAIR_1, -speed, speed)
time.sleep_ms(1050) time.sleep_ms(1050)
mp.move_tank(mp.PAIR_1, 100, 100) mp.move_tank(mp.PAIR_1, speed, speed)
time.sleep_ms(300) time.sleep_ms(300)
# Rechter Sensor sieht Grün → Rechtsabbiegen # Rechter Sensor sieht Grün → Rechtsabbiegen
elif right_color == GREEN and left_color != GREEN: 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) time.sleep_ms(700)
mp.move_tank(mp.PAIR_1, 100, -100) mp.move_tank(mp.PAIR_1, speed, -speed)
time.sleep_ms(1050) time.sleep_ms(1050)
mp.move_tank(mp.PAIR_1, 100, 100) mp.move_tank(mp.PAIR_1, speed, speed)
time.sleep_ms(300) time.sleep_ms(300)
elif right_color == BLACK and left_color == BLACK: elif right_color == BLACK or left_color == BLACK:
mp.move_tank(mp.PAIR_1, 100, 100) 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 # Linker Sensor ist Schwarz, rechter Weiß → sanft nach links
elif left_color == BLACK and right_color == WHITE: elif left_color == BLACK and right_color == WHITE:
mp.move_tank(mp.PAIR_1, -50, 50) mp.move_tank(mp.PAIR_1, speed-100, speed)
# Rechter Schwarz, linker Weiß → sanft nach rechts # Rechter Schwarz, linker Weiß → sanft nach rechts
elif right_color == BLACK and left_color == WHITE: elif right_color == BLACK and left_color == WHITE:
mp.move_tank(mp.PAIR_1, 50, -50) mp.move_tank(mp.PAIR_1, speed, speed-100)
# Beide Weiß → geradeaus (vermutlich neben der Linie) # Beide Weiß → geradeaus (vermutlich neben der Linie)
elif left_color == WHITE and right_color == WHITE: elif left_color == WHITE and right_color == WHITE:
strait = True straight = True
mp.move(mp.PAIR_1, 0, velocity=150) mp.move_tank(mp.PAIR_1, speed, speed)
runloop.sleep_ms(50)
# TODO: ---- Weiß auf beiden Seiten = Linie verloren ---- if straight:
# wenn links == weiß UND rechts == weiß: if time.ticks_diff(time.ticks_ms(), old) > 2750:
# langsam drehen bis schwarz gefunden 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: else:
old = time.ticks_ms old = time.ticks_ms()
async def main(): 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() await move_on_color()
sys.exit() sys.exit()
runloop.run(main()) runloop.run(main())