目次
#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()

トップ   編集 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS