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 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())