Compare commits
No commits in common. "master" and "feat/distanz" have entirely different histories.
master
...
feat/dista
178
src/main.llsp3
178
src/main.llsp3
|
|
@ -2,21 +2,31 @@ 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
|
||||||
|
|
||||||
BLACK = 0
|
# Implementation of Basic operations and misc. ##############################################
|
||||||
GREEN = 6
|
|
||||||
WHITE = 10
|
|
||||||
RED = 9
|
|
||||||
|
|
||||||
async def turn_right(angle=85):
|
# 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=90):
|
||||||
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=85):
|
async def turn_left(angle=90):
|
||||||
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)
|
||||||
|
|
@ -24,153 +34,91 @@ async def turn_left(angle=85):
|
||||||
# drive forward X centimeters
|
# drive forward X centimeters
|
||||||
async def drive_cm(cm, speed=200):
|
async def drive_cm(cm, speed=200):
|
||||||
# convert cm → ms (you must tune this constant!)
|
# convert cm → ms (you must tune this constant!)
|
||||||
mp.move_tank(mp.PAIR_1, -speed, -speed)
|
mp.move_tank(mp.PAIR_1, speed, speed)
|
||||||
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(speed=100):
|
async def avoid_obstacle():
|
||||||
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, -speed, -speed)
|
mp.move_tank(mp.PAIR_1, -100, -100)
|
||||||
|
|
||||||
await turn_right(85)
|
await turn_right(90)
|
||||||
await drive_cm(15)
|
await drive_cm(20)
|
||||||
await turn_left(85)
|
await turn_left(90)
|
||||||
await drive_cm(38)
|
await drive_cm(40)
|
||||||
await turn_left(85)
|
await turn_left(90)
|
||||||
while cs.reflection(port.C) > 25:
|
await drive_cm(20)
|
||||||
mp.move_tank(mp.PAIR_1, -speed, -speed)
|
await turn_right(90)
|
||||||
while cs.color(port.B) != BLACK:
|
|
||||||
mp.move_tank(mp.PAIR_1, 10, -110)
|
|
||||||
|
|
||||||
async def move_on_color():
|
async def move_on_color():
|
||||||
won = False
|
while True:
|
||||||
speed = 100
|
|
||||||
saw_green_left = False
|
|
||||||
saw_green_right = False
|
|
||||||
last_seen_black = ""
|
|
||||||
|
|
||||||
old = time.ticks_ms()
|
|
||||||
while not won:
|
|
||||||
|
|
||||||
|
|
||||||
# Farbcodes lesen
|
# Farbcodes lesen
|
||||||
left_color = cs.color(port.B)
|
left_color = cs.color(port.C)
|
||||||
right_color = cs.color(port.F)
|
right_color = cs.color(port.E)
|
||||||
front_sens = cs.reflection(port.C)
|
|
||||||
|
|
||||||
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
|
|
||||||
|
|
||||||
# Distanzsensor
|
# Distanzsensor
|
||||||
ds_front = ds.distance(port.D)
|
ds_front = ds.distance(port.F)
|
||||||
|
|
||||||
if gradient < -120:
|
|
||||||
speed = -120
|
|
||||||
elif gradient > 50:
|
|
||||||
speed = -50
|
|
||||||
else:
|
|
||||||
speed = -100
|
|
||||||
|
|
||||||
# --- LOGIK NUR MIT FARBEN ---
|
# --- LOGIK NUR MIT FARBEN ---
|
||||||
if left_color == RED or right_color == RED:
|
# Beide Sensoren sehen Grün (z.B. Kreuzung / Markierung)
|
||||||
won = True
|
if ds_front < 75:
|
||||||
|
|
||||||
|
|
||||||
elif ds_front < 75 and ds_front > -1:
|
|
||||||
await avoid_obstacle()
|
await avoid_obstacle()
|
||||||
|
|
||||||
|
elif 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
|
# Linker Sensor sieht Grün → Linksabbiegen
|
||||||
elif left_color == GREEN and right_color != GREEN:
|
elif left_color == GREEN and right_color != GREEN:
|
||||||
saw_green_left = True
|
mp.move_tank(mp.PAIR_1, 100, 100)
|
||||||
old = time.ticks_ms()
|
time.sleep_ms(700)
|
||||||
|
mp.move_tank(mp.PAIR_1, -100, 100)
|
||||||
|
|
||||||
elif right_color == GREEN and left_color != GREEN:
|
|
||||||
saw_green_right = True
|
|
||||||
old = time.ticks_ms()
|
|
||||||
|
|
||||||
# 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)
|
time.sleep_ms(1200)
|
||||||
mp.move_tank(mp.PAIR_1, speed, -speed)
|
mp.move_tank(mp.PAIR_1, 100, 100)
|
||||||
time.sleep_ms(1000)
|
|
||||||
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 saw_green_right and right_color == BLACK:
|
elif right_color == GREEN and left_color != GREEN:
|
||||||
saw_green_right = False
|
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)
|
||||||
time.sleep_ms(1200)
|
time.sleep_ms(1200)
|
||||||
mp.move_tank(mp.PAIR_1, -speed, speed)
|
mp.move_tank(mp.PAIR_1, 100, 100)
|
||||||
time.sleep_ms(1000)
|
|
||||||
mp.move_tank(mp.PAIR_1, speed, speed)
|
|
||||||
time.sleep_ms(300)
|
time.sleep_ms(300)
|
||||||
|
|
||||||
# Schwarz-Logik
|
# Linker Sensor ist Schwarz, rechter Weiß → sanft nach links
|
||||||
elif front_sens > 25 and last_seen_black == "left":
|
elif left_color == BLACK and right_color == WHITE:
|
||||||
while cs.reflection(port.C) > 25: # front_sensor
|
mp.move_tank(mp.PAIR_1, -50, 50)
|
||||||
if cs.color(port.B) == BLACK and cs.color(port.F) == WHITE: # left_color
|
|
||||||
last_seen_black = "left"
|
|
||||||
|
|
||||||
if cs.color(port.F) == BLACK and cs.color(port.B) == WHITE: # right_color
|
|
||||||
last_seen_black = "right"
|
|
||||||
break
|
|
||||||
old = time.ticks_ms()
|
|
||||||
mp.move_tank(mp.PAIR_1, -125, -20)
|
|
||||||
|
|
||||||
|
# Rechter Schwarz, linker Weiß → sanft nach rechts
|
||||||
|
elif right_color == BLACK and left_color == WHITE:
|
||||||
|
mp.move_tank(mp.PAIR_1, 50, -50)
|
||||||
|
|
||||||
elif front_sens > 25 and last_seen_black == "right":
|
# Beide Weiß → geradeaus (vermutlich neben der Linie)
|
||||||
while cs.reflection(port.C) > 25:
|
elif left_color == WHITE and right_color == WHITE:
|
||||||
if cs.color(port.F) == BLACK and cs.color(port.B) == WHITE:
|
mp.move(mp.PAIR_1, 0, velocity=150)
|
||||||
last_seen_black = "right"
|
|
||||||
|
|
||||||
if cs.color(port.B) == BLACK and cs.color(port.F) == WHITE:
|
# TODO: ---- Weiß auf beiden Seiten = Linie verloren ----
|
||||||
last_seen_black = "left"
|
# wenn links == weiß UND rechts == weiß:
|
||||||
break
|
# langsam drehen bis schwarz gefunden
|
||||||
old = time.ticks_ms()
|
|
||||||
mp.move_tank(mp.PAIR_1, -20, -125)
|
|
||||||
|
|
||||||
elif right_color == BLACK or left_color == BLACK:
|
elif gradient < -150:
|
||||||
old = time.ticks_ms()
|
mp.move(mp.PAIR_1, 0, velocity=350)
|
||||||
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 gradient > 50:
|
||||||
elif left_color == BLACK and right_color == WHITE:
|
mp.move(mp.PAIR_1, 0, velocity=100)
|
||||||
last_seen_black = "left"
|
|
||||||
|
|
||||||
# Rechter Schwarz, linker Weiß → sanft nach rechts
|
|
||||||
elif right_color == BLACK and left_color == WHITE:
|
|
||||||
last_seen_black = "right"
|
|
||||||
|
|
||||||
# Beide Weiß, Front Schwarz → geradeaus
|
|
||||||
elif cs.color(port.B) == WHITE and cs.color(port.F) == WHITE and front_sens < 25:
|
|
||||||
if time.ticks_diff(time.ticks_ms(), old) > 1000:
|
|
||||||
last_seen_black = ""
|
|
||||||
old = time.ticks_ms()
|
|
||||||
mp.move_tank(mp.PAIR_1, speed, speed)
|
|
||||||
runloop.sleep_ms(50)
|
|
||||||
|
|
||||||
async def main():
|
async def main():
|
||||||
mp.pair(mp.PAIR_1, port.A, port.E)
|
mp.pair(mp.PAIR_1, port.A, port.D)
|
||||||
await move_on_color()
|
await move_on_color()
|
||||||
|
|
||||||
sys.exit()
|
sys.exit()
|
||||||
|
|
||||||
runloop.run(main())
|
runloop.run(main())
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue