目次


課題

2017b/Mission3

製作したロボットについて

前期型

へんなアーム

最初に設計したアーム機構。
ギアをそれっぽく繋げ、ベージュギア同士で垂直方向に運動方向を変えている。
格好良いが無駄が多く、実際に動かすと様々な問題が浮上した。
一応は動くが信頼性がとにかく低く、廃止された。

アームについて

アーム

ミディアムモータ1つのみで、掴む・上げるの2つの運動が可能な機構。
真ん中の軸を回し、ベージュギアとターンテーブルギアを組み合わせ、開閉運動を生んでいる。
可動域の狭さから来る扱いにくさが玉に瑕。

機体について

ろぼっと

デフォルト機から前部を拡張し、カラーセンサ、超音波センサ、アームを追加装備している。
アーム先端の形状は、幾度もの試行錯誤によって最適化された。
各種装備によって重心が前寄りになったが、EV3を後方配置してバランスを保っている。

アームは相変わらず扱いにくいが、機体全体としてはバランスよくまとまった。

2機のルート

想定する動作、ルートについてはibuが画像付きで分かりやすく説明してくれたので参照してください。
2017b/Member/ibu/Mission3#w4c6e2cf

プログラムの説明

2機のロボット共通のプログラム

# 必要なライブラリをインポート
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 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地点から出発するロボットのプログラム

#!/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地点から出発するロボットのプログラム

#!/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
Last-modified: 2018-02-14 (水) 00:16:14 (554d)