def do_left_sharp_turn():
driver.stop()
driver.reset()
driver.lr(-SHARP_TURN, SHARP_TURN, TURNING_SPEED)
driver.block()
driver.reset()
driver.lr(FORWARD_SPEED, FORWARD_SPEED, FORWARD_SPEED)
driver.block()
def scan_lr():
driver.stop()
driver.run_to_lr(-SHARP_TURN / 2, SHARP_TURN / 2, TURNING_SPEED)
while 1:
if cs[0].is_green or cs[1].is_green:
crossing()
driver.run_to_lr(SHARP_TURN, -SHARP_TURN, TURNING_SPEED)
while 1:
if cs[0].is_green or cs[1].is_green:
crossing()
def crossing():
driver.stop()
right = 0
left = 0
back = 0
for i in range(10):
detected_green = [i.is_green() for i in cs]
if detected_green == [True, True]:
back += 1
elif detected_green == [True, False]:
left += 1
elif detected_green == [False, True]:
right += 1
print(left, back, right)
line_sensor_value = lsa.values()
if line_sensor_value[3] > THRESHOLD or line_sensor_value[4] > THRESHOLD:
if left > 8:
driver.lr(rs["color sensor y"], rs["color sensor y"],
FORWARD_SPEED)
driver.lr(-SHARP_TURN, SHARP_TURN, TURNING_SPEED)
driver.lr(FORWARD_DISTANCE, FORWARD_DISTANCE, FORWARD_SPEED)
elif right > 8:
driver.lr(rs["color sensor y"], rs["color sensor y"],
FORWARD_SPEED)
driver.lr(SHARP_TURN, -SHARP_TURN, TURNING_SPEED)
driver.lr(FORWARD_DISTANCE, FORWARD_DISTANCE, FORWARD_SPEED)
elif back > 8:
driver.lr(rs["color sensor y"], rs["color sensor y"],
FORWARD_SPEED)
driver.lr(2 * SHARP_TURN, -2 * SHARP_TURN, TURNING_SPEED)
if line_sensor_value[0] > THRESHOLD or line_sensor_value[
1] > THRESHOLD or line_sensor_value[2] > THRESHOLD:
if left > 8:
driver.lr(rs["color sensor y"], rs["color sensor y"],
FORWARD_SPEED)
driver.lr(-SHARP_TURN, SHARP_TURN, TURNING_SPEED)
driver.lr(FORWARD_DISTANCE, FORWARD_DISTANCE, FORWARD_SPEED)
elif line_sensor_value[7] > THRESHOLD or line_sensor_value[
6] > THRESHOLD or line_sensor_value[5] > THRESHOLD:
if right > 8:
driver.lr(rs["color sensor y"], rs["color sensor y"],
FORWARD_SPEED)
driver.lr(SHARP_TURN, -SHARP_TURN, TURNING_SPEED)
driver.lr(FORWARD_DISTANCE, FORWARD_DISTANCE, FORWARD_SPEED)
def centralize_lsa():
driver.stop_action = 'hold'
while 1:
vi.push(lsa.values())
if (vi.peak_pos < 0):
driver.set_speed(turning=-FC_SPD)
else:
driver.set_speed(turning=+FC_SPD)
if abs(vi.peak_pos) <= 1 and abs(vi.center_error) < 5:
driver.stop()
return
def crossing():
right = 0
left = 0
back = 0
for i in range(10):
detected_green = [i.is_green() for i in cs]
if detected_green == [True, True]:
back += 1
elif detected_green == [True, False]:
left += 1
elif detected_green == [False, True]:
right += 1
print(left, right, back)
if left > 8:
driver.stop()
driver.reset()
driver.lr(rs["color sensor y"], rs["color sensor y"], FORWARD_SPEED)
driver.block()
driver.reset()
driver.lr(-SHARP_TURN, SHARP_TURN, TURNING_SPEED)
driver.block()
driver.reset()
driver.lr(0.5, 0.5, FORWARD_SPEED)
elif right > 8:
driver.stop()
driver.reset()
driver.lr(rs["color sensor y"], rs["color sensor y"], FORWARD_SPEED)
driver.block()
driver.reset()
driver.lr(SHARP_TURN, -SHARP_TURN, TURNING_SPEED)
driver.block()
driver.reset()
driver.lr(0.5, 0.5, FORWARD_SPEED)
elif back > 8:
driver.stop()
driver.reset()
driver.lr(rs["color sensor y"], rs["color sensor y"], FORWARD_SPEED)
driver.stop()
driver.reset()
driver.lr(2 * SHARP_TURN, -2 * SHARP_TURN, TURNING_SPEED)
def centralize_front():
driver.set_speed(turning=5)
while lsa.line_pos != 0:
print(lsa.line_pos)
driver.stop()
return
def s():
d.stop()
def run():
try:
m.update_loop()
except KeyboardInterrupt:
d.stop()
def stop_both():
d.stop()
