alles funktioniert (nicht)
parent
ef4db27b62
commit
b7d96846f0
|
|
@ -20,6 +20,11 @@ import time
|
|||
|
||||
# Productive Code ###########################################################################
|
||||
|
||||
BLACK = 0
|
||||
GREEN = 6
|
||||
WHITE = 10
|
||||
RED = 9
|
||||
|
||||
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!
|
||||
|
|
@ -33,7 +38,7 @@ async def turn_left(angle=85):
|
|||
# drive forward X centimeters
|
||||
async def drive_cm(cm, speed=200):
|
||||
# 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)
|
||||
mp.stop(mp.PAIR_1)
|
||||
|
||||
|
|
@ -45,28 +50,30 @@ async def avoid_obstacle(speed=100):
|
|||
await turn_right(85)
|
||||
await drive_cm(15)
|
||||
await turn_left(85)
|
||||
await drive_cm(30)
|
||||
await drive_cm(38)
|
||||
await turn_left(85)
|
||||
await drive_cm(15)
|
||||
await turn_right(85)
|
||||
old = 0
|
||||
while cs.reflection(port.C) > 25:
|
||||
mp.move_tank(mp.PAIR_1, -speed, -speed)
|
||||
while cs.color(port.B) != BLACK:
|
||||
mp.move_tank(mp.PAIR_1, 10, -110)
|
||||
#await turn_right(85)
|
||||
async def move_on_color():
|
||||
won = False
|
||||
old = time.ticks_ms()
|
||||
speed = 100
|
||||
saw_green_left = False
|
||||
saw_green_right = False
|
||||
last_seen_black = ""
|
||||
|
||||
old = time.ticks_ms()
|
||||
# print("initial old: " + str(old))
|
||||
while not won:
|
||||
|
||||
|
||||
|
||||
# Farbcodes lesen
|
||||
left_color = cs.color(port.B)
|
||||
right_color = cs.color(port.F)
|
||||
front_sens = cs.reflection(port.C)
|
||||
|
||||
|
||||
gradient = ms.tilt_angles()[2]
|
||||
# Farbcodes
|
||||
BLACK = 0
|
||||
|
|
@ -92,8 +99,7 @@ async def move_on_color():
|
|||
|
||||
|
||||
elif ds_front < 75 and ds_front > -1:
|
||||
#await avoid_obstacle()
|
||||
pass
|
||||
await avoid_obstacle()
|
||||
|
||||
# Linker Sensor sieht Grün → Linksabbiegen
|
||||
elif left_color == GREEN and right_color != GREEN:
|
||||
|
|
@ -128,18 +134,18 @@ async def move_on_color():
|
|||
mp.move_tank(mp.PAIR_1, speed, speed)
|
||||
time.sleep_ms(300)
|
||||
|
||||
# Schwar-Logik
|
||||
# Schwarz-Logik
|
||||
elif front_sens > 25 and last_seen_black == "left":
|
||||
#time.sleep_ms(100)
|
||||
while cs.reflection(port.C) > 25:
|
||||
if cs.color(port.B) == BLACK and cs.color(port.F) == WHITE:
|
||||
while cs.reflection(port.C) > 25: # front_sensor
|
||||
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:
|
||||
if cs.color(port.F) == BLACK and cs.color(port.B) == WHITE: # right_color
|
||||
last_seen_black = "right"
|
||||
#mp.move_tank(mp.PAIR_1, speed, speed)
|
||||
#time.sleep_ms(1000)
|
||||
break
|
||||
mp.move_tank(mp.PAIR_1, -75, 50)
|
||||
mp.move_tank(mp.PAIR_1, -125, -20)
|
||||
|
||||
|
||||
elif front_sens > 25 and last_seen_black == "right":
|
||||
|
|
@ -152,7 +158,7 @@ async def move_on_color():
|
|||
#mp.move_tank(mp.PAIR_1, speed, speed)
|
||||
#time.sleep_ms(1000)
|
||||
break
|
||||
mp.move_tank(mp.PAIR_1, 50, -75)
|
||||
mp.move_tank(mp.PAIR_1, -20, -125)
|
||||
|
||||
elif right_color == BLACK or left_color == BLACK:
|
||||
if right_color == BLACK and left_color == BLACK:
|
||||
|
|
@ -169,27 +175,19 @@ async def move_on_color():
|
|||
elif right_color == BLACK and left_color == WHITE:
|
||||
last_seen_black = "right"
|
||||
|
||||
# Beide Weiß → geradeaus (vermutlich neben der Linie)
|
||||
elif left_color == WHITE and right_color == WHITE and front_sens < 25:
|
||||
# Beide Weiß, Front Schwarz → geradeaus
|
||||
elif cs.color(port.B) == WHITE and cs.color(port.F) == WHITE and front_sens < 25:
|
||||
straight = True
|
||||
mp.move_tank(mp.PAIR_1, speed, speed)
|
||||
runloop.sleep_ms(50)
|
||||
|
||||
|
||||
if straight:
|
||||
if time.ticks_diff(time.ticks_ms(), old) > 200:
|
||||
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
|
||||
if front_sens > 25:
|
||||
last_seen_black = ""
|
||||
else:
|
||||
old = time.ticks_ms()
|
||||
# if time.ticks_diff(time.ticks_ms(), old) > 5000:
|
||||
# print(time.ticks_diff(time.ticks_ms(), old))
|
||||
# straight = True
|
||||
|
||||
# Alles Weiß
|
||||
# elif cs.color(port.B) == WHITE and cs.color(port.F) == WHITE and front_sens > 25:
|
||||
|
||||
async def main():
|
||||
mp.pair(mp.PAIR_1, port.A, port.E)
|
||||
|
|
|
|||
Loading…
Reference in New Issue