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