目次 #contents ------- *課題 [#w4f86c70] [[2017b/Mission3]] *製作したロボットについて [#v97d36a9] **前期型 [#f9e9eb0f] #ref(2017b/Member/ibu/Mission3/proto_armA-1.jpg,80%,へんなアーム) 最初に設計したアーム機構。~ ギアをそれっぽく繋げ、ベージュギア同士で垂直方向に運動方向を変えている。~ 格好良いが無駄が多く、実際に動かすと様々な問題が浮上した。~ 一応は動くが信頼性がとにかく低く、廃止された。 **アームについて [#o19dd2ac] #ref(2017b/Member/ibu/Mission3/proto_armC.jpg,100%,アーム) ミディアムモータ1つのみで、掴む・上げるの2つの運動が可能な機構。~ 真ん中の軸を回し、ベージュギアとターンテーブルギアを組み合わせ、開閉運動を生んでいる。~ 可動域の狭さから来る扱いにくさが玉に瑕。~ **機体について [#n128c555] #ref(2017b/Member/ibu/Mission3/fin_machines.jpg,100%,ろぼっと) デフォルト機から前部を拡張し、カラーセンサ、超音波センサ、アームを追加装備している。~ アーム先端の形状は、幾度もの試行錯誤によって最適化された。~ 各種装備によって重心が前寄りになったが、EV3を後方配置してバランスを保っている。~ アームは相変わらず扱いにくいが、機体全体としてはバランスよくまとまった。 *2機のルート [#t1b5fcba] 想定する動作、ルートについてはibuが画像付きで分かりやすく説明してくれたので参照してください。~ [[2017b/Member/ibu/Mission3#w4c6e2cf]] *プログラムの説明 [#m2b714b1] **2機のロボット共通のプログラム [#x8477aec] # 必要なライブラリをインポート 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' # 超音波センサーのインスタンスを作成、距離(cm)を計測するモードに変更 us = ev3.UltrasonicSensor() assert us.connected, "Connect a color sensor to any sensor port" assert us.connected, "Connect an ultrasonic sensor to any sensor port" us.mode = 'US-DIST-CM' position_sp_to_angle = 1.55 # モータの回転角度で、車体の回転角度(°)を求めるときに使う target_val = 50 # 反射光量の目標値(ラインの境界) std_speed = 22 # 移動の標準スピード kp = std_speed*0.04 # ライントレースの比例制御の定数 position_sp_to_cm = 20.05 # モータの回転角度で、車体の移動距離(cm)を求めるときに使う intersection_time = 0.23/1.8 # 交差点と判断するときの秒数 angle_stabilization_time = 0.3*1.8 # ラインと車体が並行になるために必要な時間 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 arm_open(): 'アームを開く' arm.run_to_abs_pos(position_sp=200, speed_sp=std_speed*5, stop_action="hold") arm.wait_while("running") def cup_catch(): 'コップを掴む' mr.run_forever(speed_sp=std_speed*5) ml.run_forever(speed_sp=std_speed*5) while us.value() > 50: pass for motor in mr, ml: motor.stop(stop_action='brake') move(5) arm.run_to_abs_pos(position_sp=0, speed_sp=std_speed*5, stop_action="hold") arm.wait_while("running") move(-5) def cup_a_little_lift(): 'コップをちょっと持ち上げる。' arm.run_to_abs_pos(position_sp=-42, speed_sp=std_speed*5, stop_action="hold") arm.wait_while("running") def cup_lift(): 'コップを大きく持ち上げる' arm.run_to_abs_pos(position_sp=-230, speed_sp=std_speed*5, stop_action="hold") arm.wait_while("running") def search_min_d(search_time=4.5, i=1): ''' 障害物の最短距離を探してその方向に向くための関数。 iが1のとき反時計回りで、iが-1のとき時計回りで探す。 デフォルトの4.5秒で周囲一周分を探すことができる。 ''' if i == 1: signs = [1, -1] elif i == -1: signs = [-1, 1] min_d = 500 # 最短距離の初期値を50cmに設定 start_time = time.time() mr.run_forever(speed_sp=signs[0]*std_speed*5) ml.run_forever(speed_sp=signs[1]*std_speed*5) while time.time()-start_time < search_time: # 指定時間の間回転し、最短距離を更新 if min_d > us.value(): min_d = us.value() for motor in mr, ml: motor.stop(stop_action='brake') start_time = time.time() mr.run_forever(speed_sp=signs[1]*std_speed*5) ml.run_forever(speed_sp=signs[0]*std_speed*5) while time.time()-start_time < search_time: # 前に探した範囲で、最短距離が見つかったら停止 if us.value() <= min_d: for motor in mr, ml: motor.stop(stop_action='brake') return # 見つかれば終了 for motor in mr, ml: motor.stop(stop_action='brake') search_min_d(search_time) # 見つからなければ、もう一度はじめからやり直す。 def line_adjust_by_edge(edge): '交差点の誤検知を避けるために使う関数' for motor in mr, ml: motor.run_direct() while abs(cs.value()-target_val) > 10: line_following(edge) for motor in mr, ml: motor.stop(stop_action='brake') 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') def run_until_line(): 'ラインが見つかるまで走る' for motor in mr, ml: motor.run_forever(speed_sp=std_speed*5) while abs(cs.value()-target_val) > 10: pass for motor in mr, ml: motor.stop(stop_action='brake') **A地点から出発するロボットのプログラム [#n55623be] #!/usr/bin/env python3 from def_green_v016 import * ''' 課題3、緑の目印がついたロボット用。 A地点を出発する。 ''' ''' # D地点から出発するロボットと通信するコード # 機体が完成したのが前日夜で、テストする時間がなかったため、発表では使わなかった。 import paho.mqtt.client as mqtt client = mqtt.Client() # MQTTのクライアントを生成 position = 0 def on_connect(client, userdata, flags, rc): # ブローカに接続した時に実行される関数(コールバック関数)を定義しておく print("Connected with result code "+str(rc)) client.subscribe("position") # "position"というトピックスを購読 def on_message(client, userdata, msg): # メッセージを受け取った時に実行される関数(コールバック関数)を定義しておく position = int(msg.payload) # payloadという変数にメッセージが入る print(position) ''' def arm_set(): 'アームの位置を調整するための関数' a = input('w, s') while a in {'w', 's'}: if a == 'w': arm.run_timed(time_sp=0.25*1000, speed_sp=200, stop_action='brake') arm.wait_while('running') elif a == 's': arm.run_timed(time_sp=0.25*1000, speed_sp=-200, stop_action='brake') arm.wait_while('running') a = input('w, s') def main(): ''' # D地点から出発するロボットと通信するコード # 機体が完成したのが前日夜で、テストする時間がなかったため、発表では使わなかった。 client.on_connect = on_connect # 上で定義したコールバック関数を渡す client.on_message = on_message # 上で定義したコールバック関数を渡す client.connect("localhost",1883, 60) # ブローカ(自分自身なのでlocalhost)の1883番ポートに接続 (Keep Alive は60秒) client.loop_start() # メッセージ受信ループの開始 ''' run_until_line() move(4) line_adjust_by_edge('right_edge') run_until_intersection('right_edge') circle(33) arm_open() search_min_d(0.8) cup_catch() circle(-160) line_adjust_by_edge('left_edge') run_until_intersection('left_edge') intersection_turn_left() line_adjust_by_time('left_edge', 1.3) cup_a_little_lift() move(5) cup_lift() move(-50) ''' # D地点から出発するロボットと通信するコード # 機体が完成したのが前日夜で、テストする時間がなかったため、発表では使わなかった。 line_adjust_by_time('left_edge', 5) run_until_intersection('left_edge') while position != 1: pass move(15) circle(35*1.2) line_adjust_by_edge('left_edge') line_adjust_by_time('left_edge', 3) run_until_intersection('left_edge') intersection_turn_left() run_until_intersection('left_edge') circle(-27) arm_open() client.loop_stop() # 受信ループを停止 client.disconnect() # 接続を切断 ''' if __name__ == '__main__': arm_set() init_arm() main() **D地点から出発するロボットのプログラム [#n0c7756f] #!/usr/bin/env python3 from def_red_v016 import * ''' 課題3、緑の目印がついたロボット用。 A地点を出発する。 ''' ''' # A地点から出発するロボットと通信するコード # 機体が完成したのが前日夜で、テストする時間がなかったため、発表では使わなかった。 import paho.mqtt.client as mqtt # pahoのライブラリをインポート client = mqtt.Client() # MQTTのクライアントを生成 ''' def arm_set(): a = input('w, s') while a in {'w', 's'}: if a == 'w': arm.run_timed(time_sp=0.1*1000, speed_sp=200, stop_action='brake') arm.wait_while('running') elif a == 's': arm.run_timed(time_sp=0.1*1000, speed_sp=-200, stop_action='brake') arm.wait_while('running') a = input('w, s') ''' # A地点から出発するロボットと通信するコード # 機体が完成したのが前日夜で、テストする時間がなかったため、発表では使わなかった。 def mqtt_send(x): #値を送信 position = x #位置情報をアルファベットで送信 client.publish("position", position) # "position"というトピックでブローカに送る print("send_position_"+x) ''' def main(): ''' # A地点から出発するロボットと通信するコード # 機体が完成したのが前日夜で、テストする時間がなかったため、発表では使わなかった。 #client.connect("10.60.2.156",1883, 60) # ブローカに接続、(60秒以上接続がないと切断) ''' line_adjust_by_time('left_edge', 1.3) run_until_intersection('left_edge') intersection_turn_left() change_following_edge() run_until_intersection('right_edge') intersection_turn_right() line_adjust_by_time('right_edge', 1) run_until_intersection('right_edge') circle(145) # intersection,change direction arm_open() move(-4) search_min_d(1) cup_catch() circle(-105) move(1) line_adjust_by_time('left_edge', 1) circle(-20) run_until_intersection('left_edge') cup_a_little_lift() move(4) circle(35) move(5) cup_a_little_lift() move(10) cup_lift() circle(-30) move(-5) search_min_d(1, -1) cup_catch() arm_open() cup_a_little_lift() circle(60) cup_lift() circle(-140) arm_open() move(-4) circle(-90) #mqtt_send(1) move(50) ''' # A地点から出発するロボットと通信するコード # 機体が完成したのが前日夜で、テストする時間がなかったため、発表では使わなかった。 #client.disconnect() # 切断 ''' if __name__ == '__main__': arm_set() init_arm() main()