目次
2017b/Mission2 コース1を選択。
デフォルトの車体に、カラーセンサとミッション1の機構をそのままに取り付けた。
カラーセンサの位置決定には時間を費やし、ライントレースに最適な位置に設置した。
また、カラーセンサの精度を高めるため、ブルドーザーを参考に整地用ブレードを取り付けている。
アームは前回のX軸制御機構をそのまま流用。
アームの幅が大きすぎるため、走行時にコップを弾き、目標のコップをつかめないことがあった。
むつかしいことは考えず、回転運動をそのままアームに伝えるシンプルな構造とした。
これにより、コップを弾いてしまう問題は解決された。
import ev3dev.ev3 as ev3 import time
# assertで接続出来てないときはどこが接続できていないかメッセージを表示して、プログラムを停止する。 # どのモータが接続できていないのか分かりやすくなる。 # 右タイヤに繋がっているモータのインスタンスを作成 mr = ev3.LargeMotor('outC') assert mr.connected, "Connect right large motor to port C" # 左タイヤに繋がっているモータのインスタンスを作成 ml = ev3.LargeMotor('outD') assert ml.connected, "Connect left large motor to port A" # コップを掴み、上げ下げするための機構に繋がるモータのインスタンスを作成 arm = ev3.MediumMotor('outB') assert arm.connected, "Connect arm medium motor to port B" # カラーセンサのインスタンスを作成、反射光の量(0~100)を測定するモードに変更 cs = ev3.ColorSensor() assert cs.connected, "Connect a color sensor to any sensor port" cs.mode = 'COL-REFLECT'
position_sp_to_angle = 17/9 # モータの回転角度で、車体の回転角度(°)を求めるときに使う target_val = 50 # 反射光量の目標値(ラインの境界) std_speed = 40 # 移動の標準スピード kp = std_speed*0.04 # ライントレースの比例制御の定数 position_sp_to_cm = 20.05 # モータの回転角度で、車体の移動距離(cm)を求めるときに使う intersection_time = 0.23 # 交差点と判断するときの秒数 angle_stabilization_time = 0.3 # ラインと車体が並行になるために必要な時間
def run_until_intersection(edge): '交差点が見つかるまで走り、見つけたらビープ音を鳴らして止まる' global global_edge # global_edgeをグローバル変数として宣言 global_edge = edge # 引数のedgeをグローバル変数にする start_time = time.time() for motor in mr, ml: motor.run_direct() # 交差点と判断するまでライントレースを続ける while time.time()-start_time <= intersection_time: line_following(edge) if 10 < cs.value() < 100: # ラインの外にきたら測定している時間をリセット start_time = time.time() ev3.Sound.beep() while abs(cs.value()-target_val) > 10: # 目標値に近づける line_following(edge) for motor in mr, ml: motor.stop(stop_action='brake')
def line_following(edge): 'ライントレースのベースとなる関数' if edge == 'left_edge': p = (cs.value()-target_val)*kp elif edge == 'right_edge': p = (target_val-cs.value())*kp else: print('invalid edge') if p > 0: mr.duty_cycle_sp = std_speed-p ml.duty_cycle_sp = std_speed elif p <= 0: mr.duty_cycle_sp = std_speed ml.duty_cycle_sp = std_speed+p
def change_following_edge(): 'トレースする線を切り替える関数' line_adjust_by_time(global_edge) if global_edge == 'right_edge': circle(90) move(2) circle(-90) elif global_edge == 'left_edge': circle(-90) move(2) circle(90)
def move(distance): '車体の前後移動を行う関数。距離(cm)で指定できる。後退するときは負の値を使う' for motor in mr, ml: motor.run_to_rel_pos(position_sp=distance*position_sp_to_cm, speed_sp=std_speed*5, stop_action='brake') for motor in mr, ml: motor.wait_while('running')
def circle(rotation_angle): '車体の回転を行う関数。角度角度を指定できる。半時計回りが正' if rotation_angle < 0: signs = [-1, 1] elif rotation_angle >= 0: signs = [1, -1] for motor, sign in zip([mr, ml], signs): motor.run_to_rel_pos(position_sp=sign*abs(rotation_angle)*position_sp_to_angle, speed_sp=std_speed*5, stop_action='brake') for motor in mr, ml: motor.wait_while('running')
def intersection_go_straight(): '交差点を見つけたとき、直進するための関数' if global_edge == 'left_edge': circle(-70*1.2) elif global_edge == 'right_edge': circle(70*1.2) for motor in mr, ml: motor.run_direct() while abs(cs.value()-target_val) > 10: line_following(global_edge) for motor in mr, ml: motor.stop(stop_action='brake')
def intersection_turn_right(): '交差点を見つけたとき、右折するための関数' if global_edge == 'left_edge': circle(-145) move(2)
def intersection_turn_left(): '交差点を見つけたとき、左折するための関数' if global_edge == 'right_edge': circle(145) move(2)
def init_arm(): 'アームの初期化を行うための関数。アームが閉じた状態を初期値にすることで、他のarm系統の関数が使用できる' arm.reset() # del stop_action='hold' input('Press Enter to initialize closed arm position') arm.reset()
def close_arm(): 'アームを閉じる' arm.run_to_abs_pos(position_sp=0, speed_sp=std_speed*5/2, stop_action="hold") arm.wait_while("running")
def open_arm(): 'アームを開く' arm.run_to_abs_pos(position_sp=-80, speed_sp=std_speed*5/2, stop_action="hold") arm.wait_while("running")
def line_adjust_by_time(edge, stab_time=angle_stabilization_time): '線と車体を並行にするための関数。交差点を検知したあとなどに車体が斜めになるので作った' for motor in mr, ml: motor.run_direct() start_time = time.time() while time.time()-start_time < stab_time: line_following(edge) for motor in mr, ml: motor.stop(stop_action='brake')
#!/usr/bin/env python3 from tracer_v176 import *
init_arm()
# start car sound # Aをスタート run_until_intersection('right_edge') intersection_turn_right() run_until_intersection('right_edge')
# Bを直進 intersection_go_straight() run_until_intersection('right_edge')
# Cを右折,Fを直進 intersection_turn_right() change_following_edge() # avoid misdetection of sharp turn as intersection for motor in mr, ml: motor.run_direct() start_time = time.time() while time.time()-start_time < 14: line_following('left_edge') for motor in mr, ml: motor.stop(stop_action='brake') open_arm() run_until_intersection('left_edge')
time.sleep(1) # 一時停止
# X地点の紙コップを取得 close_arm()
# Rを左折 intersection_turn_left() run_until_intersection('left_edge')
# Pを直進 intersection_go_straight() run_until_intersection('left_edge')
# Qを左折 intersection_turn_left() run_until_intersection('left_edge')
# Sを直進(一時停止) time.sleep(1) intersection_go_straight() # avoid misdetection of sharp turn as intersection for motor in mr, ml: motor.run_direct() start_time = time.time() while time.time()-start_time < 6: line_following('left_edge') for motor in mr, ml: motor.stop(stop_action='brake') run_until_intersection('left_edge')
# Sを直進(一時停止) time.sleep(1)
# Y地点に紙コップを置いてコースに戻る circle(75) move(7) open_arm() move(-7) circle(-75) intersection_go_straight() run_until_intersection('left_edge') close_arm() arm.reset()
# Fを左折(一時停止) time.sleep(1) intersection_turn_left() run_until_intersection('left_edge')
# Cを右折(一時停止) time.sleep(1) intersection_turn_right()
# D地点へ(ゴール) move(16)