2017b/Member/Koyo/Mission3
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
検索
|
最終更新
|
ヘルプ
|
ログイン
]
開始行:
目次
#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%,アーム)
ミディアムモータ１つのみで、掴む・上げるの２つの運動が可能な機構。~
真ん中の軸を回し、ベージュギアとターンテーブルギアを組み合わせ、開閉運動を生んでいる。~
可動域の狭さから来る扱いにくさが玉に瑕。~
**機体について [#n128c555]
#ref(2017b/Member/ibu/Mission3/fin_machines.jpg,100%,ろぼっと)
デフォルト機から前部を拡張し、カラーセンサ、超音波センサ、アームを追加装備している。~
アーム先端の形状は、幾度もの試行錯誤によって最適化された。~
各種装備によって重心が前寄りになったが、EV3を後方配置してバランスを保っている。~
アームは相変わらず扱いにくいが、機体全体としてはバランスよくまとまった。
*２機のルート [#t1b5fcba]
想定する動作、ルートについてはibuが画像付きで分かりやすく説明してくれたので参照してください。~
[[2017b/Member/ibu/Mission3#w4c6e2cf]]
*プログラムの説明 [#m2b714b1]
**２機のロボット共通のプログラム [#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 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 *
'''
課題３、緑の目印がついたロボット用。
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 *
'''
課題３、緑の目印がついたロボット用。
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()
終了行:
目次
#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%,アーム)
ミディアムモータ１つのみで、掴む・上げるの２つの運動が可能な機構。~
真ん中の軸を回し、ベージュギアとターンテーブルギアを組み合わせ、開閉運動を生んでいる。~
可動域の狭さから来る扱いにくさが玉に瑕。~
**機体について [#n128c555]
#ref(2017b/Member/ibu/Mission3/fin_machines.jpg,100%,ろぼっと)
デフォルト機から前部を拡張し、カラーセンサ、超音波センサ、アームを追加装備している。~
アーム先端の形状は、幾度もの試行錯誤によって最適化された。~
各種装備によって重心が前寄りになったが、EV3を後方配置してバランスを保っている。~
アームは相変わらず扱いにくいが、機体全体としてはバランスよくまとまった。
*２機のルート [#t1b5fcba]
想定する動作、ルートについてはibuが画像付きで分かりやすく説明してくれたので参照してください。~
[[2017b/Member/ibu/Mission3#w4c6e2cf]]
*プログラムの説明 [#m2b714b1]
**２機のロボット共通のプログラム [#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 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 *
'''
課題３、緑の目印がついたロボット用。
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 *
'''
課題３、緑の目印がついたロボット用。
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()
ページ名:
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
