2019b/Member

課題3

青と赤のボールを運搬して、空き缶の上に載せる。

2019b-mission2.png

フィールドの説明

  • フィールドは課題2で使用した紙を使用する。
  • 空き缶を6本用意し、そのうち3本をC,E,Fの交差点に描いた半径4cmの円内に置き、 その上にボールを載せておく。
  • 同様に残りの空き缶をC', E', F' 地点に置く。

ロボットの説明 ハード面

このロボットのコンセプトとしてはカラクリをイメージしました。

14624_2.jpg

画像のようにロボットの機体の中にボールを通すことで缶にボールを置く仕組みになっています。この形はロボットを製作する際、できたら面白いということでこの形になりました。

このロボットは一機で二つのEV3を使い、三つのボールを取り、溜めて、三つ缶に置くというのを目的にしました。

アーム

14648_2.jpg

このアームはボールを取るために作りました。このアームは人間の腕の肘、手首、掴む手を模しています。

このアームはモータを3つ使用してますが、モータを関節部分につけてしまうと前重心になりすぎるのでモータを2つ中心に寄せ、ギアによって力を伝える機構にしました。これにより前重心になりすぎるのを軽減しました。ギアを使用する際、組み合わせ方が難しく苦戦しました。

このアームは重量が重たいので前重心になっているので後ろを重くする必要がありました。

ボールの認識は超音波センサではなくカラーセンサを使用しました超音波センサだとどうしてもサイズが大きくなってしまい邪魔になるのでカラーセンサになりました。

送り出し部分

14625_2.jpg

この送り出し部分ではモータを一つ使用しています。ここでは溜めておいたボールを一つずつ送り出すために設計されています。120度ごとに区切っておりボールを一つ送るのに120度回します。

カコイ

14634_2.jpg

転がってきたボールが缶から出ないようアーム部分にカコイを付けました。これが壁になり、勢いを殺し、缶にしっかりボールが乗ります。

実際のプログラム

test3_armEV3.py

#!/usr/bin/env python3
       from ev3dev2.motor import OUTPUT_B,OUTPUT_C,MoveTank,LargeMotor,Motor,OUTPUT_A,OUTPUT_D
       from ev3dev2.sensor.lego import ColorSensor,TouchSensor,UltrasonicSensor
       import paho.mqtt.client as mqtt
       from time import sleep

       m_e = LargeMotor(OUTPUT_B)
       m_w = LargeMotor(OUTPUT_A)
       m_set_e = MoveTank(OUTPUT_B,OUTPUT_A)
       m_h = Motor(OUTPUT_C)

       v_e = 10
       v_w = 10
       v_h = 5

       m_h.on(0)
       m_w.on(0)
       m_e.on(0)

       t_wait = 0.5

class joint:
       def __init__(self,part,deg_up,deg_set,deg_down,v): #part=モーター(m_eまたはm_wまたはm_h),deg_up=up()実行時にpartで指定したモーターの角度を何度にするか,deg_down=down()実行時にpartで指定したモーターの角度を何度にするか,deg_set=set()実行時にpartで指定したモーターの角度を何度にするか
       self.deg_up = deg_up
       self.deg_set = deg_set
       self.deg_down = deg_down
       self.part = part
       self.v = v

       def up(self): #partで指定したモーターの角度を、アームがボールを持ち上げたときの角度にする
       self.part.on_to_position(self.v,self.deg_up,block=True)
       def set(self): #partで指定したモーターの角度を、ボールを置くときの角度にする
       self.part.on_to_position(self.v,self.deg_set,block=True)
       def down(self): #partで指定したモーターの角度を、ボールを探すときの角度にする
       self.part.on_to_position(self.v,self.deg_down,block=True)

       elbow = joint(m_e,0,-165,-200,v_e)
       wrist = joint(m_w,140,-150,-130,v_w)
       hand = joint(m_h,0,100,10,v_w)

       #-----------------------------------------------------
       t_reset=1
       v_e_reset = 10
       v_w_reset = 20
       v_h_reset = 10

       def on_until_stalled(m,v): #モーターが止まるまで回す関数,m=モーター(m_eまたはm_wまたはm_h),v=モーターを回す速度
       m.on(v)
       sleep(t_reset)
       while m.speed>0:
       m.on(v)
       def arm_reset(): #アームがある程度どんな形でも特定の形にしてアームのモーターの角度を全てリセットする関数
       m_set_e.on_for_degrees(v_e_reset,v_e_reset,90)
       #hand
       print("hand")
       on_until_stalled(m_h,-v_h_reset)
       m_h.on(0)
       #wrist
       print("wrist")
       m_e.stop(stop_action="hold")
       on_until_stalled(m_w,v_w_reset)
       m_w.on(0)
       m_w.stop(stop_action='hold')
       #elbow
       print("elbow")
       on_until_stalled(m_e,v_e_reset)
       m_w.stop(stop_action='coast')
       m_set_e.on_for_degrees(-v_e_reset,-v_e_reset,45)
       m_e.stop(stop_action='hold')
       on_until_stalled(m_w,-v_w_reset)
       m_w.on(0)
       m_w.stop(stop_action='hold')
       m_e.on_for_degrees(v_e_reset,45)
       m_w.stop(stop_action='coast')
       on_until_stalled(m_e,v_e_reset)
       #reset
       m_h.on(0)
       m_w.on(0)
       m_e.on(0)
       sleep(t_reset)
       m_h.reset()
       m_w.reset()
       m_e.reset()
       #------------------------------
       def fetch_ball(): #ボールをつかむ関数
       hand.up()
       sleep(t_wait)
       wrist.up()
       sleep(t_wait)
       elbow.up()
       sleep(t_wait)
       hand.set()
       reset()


       def set_arm(): #アームをボールを探すときのアームの形にする関数
       hand.set()
       sleep(t_wait)
       wrist.set()
       sleep(t_wait)
       elbow.set()


       def down_arm(): #アームをボールを置くときのアームの形にする関数
       hand.down()
       sleep(t_wait)
       elbow.down()
       sleep(t_wait)
       wrist.down()


       #-----------------------------

       def on_connect(client,userdata,flags,rc):
       print("Connected with result code:" +str(rc))
       client.subscribe("arm")

       def on_message(client,userdata,message):
       mes_str =  message.payload.decode()
       print("message topic:" + message.topic)
       print("message:" + mes_str)

       if mes_str=="fetch_ball":
       fetch_ball()
       elif mes_str=="set_arm":
       set_arm()
       elif mes_str=="reset_arm":
       reset_arm()
       elif mes_str=="down_arm":
       down_arm()

       def main():
       address = input("address:")
       client = mqtt.Client()
       client.on_connect = on_connect
       client.on_message = on_message
       client.connect(address,1883,60)

       client.loop_start()
       main_bool = True
       while main_bool:
       a=input()
       main_bool = a!="stop"
       print("end")
       client.loop_stop()
       client.disconnect()

       if __name__ == "__main__":
       main()

test3_mainEV3 .py

#!/usr/bin/env python3
       from ev3dev2.motor import OUTPUT_B,OUTPUT_C,MoveTank,LargeMotor,Motor,OUTPUT_A
       from ev3dev2.sensor.lego import ColorSensor,TouchSensor,UltrasonicSensor
       from ev3dev2.sensor import INPUT_1,INPUT_4
       import paho.mqtt.client as mqtt
       from time import sleep

       address = input("address:")

       m_r = LargeMotor(OUTPUT_C)
       m_l = LargeMotor(OUTPUT_B)
       tank = MoveTank(OUTPUT_B,OUTPUT_C)
       cs = ColorSensor(INPUT_4)
       cs2 = ColorSensor(INPUT_1)
       ts = TouchSensor()
       us = UltrasonicSensor()
       m_stocker = Motor(OUTPUT_A)
       v_stocker = 5

       threshold = 45
       v_s = 12.5
       v_w = 5.0
       v_b = 10
       k_s = 0.25
       k_w = 1.5
       k_b = 0.75
       cs_val_black = 10
       cs_th_d_max = 20
       csd_s_max = 3

       v_is = 5

       cs.mode ="COL-REFLECT"
       cs2.mode ="RGB-RAW"
       us.mode ="US-DIST-CM"

       a1_is=-0.15
       a2_is=1.2

       v_s_fb = 12.5
       v_w_fb = 5
       v_b_fb = 10
       k_s_fb = 0.25
       k_w_fb = 1.75
       k_b_fb = 0.75

       v_s_pb = 12.5
       v_w_pb = 5.0
       v_b_pb = 10
       k_s_pb = 0.25
       k_w_pb = 2.25
       k_b_pb = 0.75

       #1-------------------------------------------------------------------------------------

       def read_list(file_name): #file_nameで指定されたファイルを読み込み、listにまとめる関数,file_name=ファイルの名前
       main_break_bool =  False
       list=[]
       try:
       with open(file_name, "r") as f:
       list_raw = f.readlines()
       for i in list_raw:
       if i.strip() != '':
       list.append(i.split(","))
       for i in range(len(list)):
       for j in range(len(list[i])):
       list[i][j] = list[i][j].strip()
       list.append("end")
       print("raw list:",list_raw)
       print("list:",list)
       print("")
       #if not (check_list_bool(list)):
       #    print("file content error")
       #    raise Exception
       except:
       print("file read error")
       main_break_bool=True
       return (list,main_break_bool) #list=交差点または急なカーブに差し掛かったときに何をするかを表すリスト,main_break_bool=ファイル読み込みでエラーがあればTrue、なければFalseをとる変数

       #2---intersection----------------------------------------------------------------------------------

       def intersection(sign,list): #交差点や急なカーブに差し掛かった時listとsignによって指定された道の指定された境界に行く関数,sign=整数(-1または1)=intersection()実行までにとっていた境界が右か左かを表す数字(右なら1左なら-1をとる),list=["i",整数,"r"または"l"]=[intersection()を実行することをあらわす文字,選ぶ道が何番目にあるかを表す数字(分岐を中心として、右の境界をとっていたとき時計回りに、左の境界をとっていたとき反時計回りに数える),選ぶ道の右と左の境界のうちどちらをとるか]
       print("list:",list)
       n_w = int(list[1])  # number

       if list[2]=="r":    # sign
       sign_is = 1
       elif list[2]=="l":
       sign_is = -1

       tank.on((a1_is+sign)*v_is,(a1_is-sign)*v_is)
       while True:
       if cs.value()>threshold:
       break

       tank.on((a2_is-sign)*v_is,(a2_is+sign)*v_is)
       for i in range(n_w):
       while True:
       if cs.value()<=cs_val_black:
       break
       while True:
       if cs.value()>threshold:
       break
       if sign_is*sign==1:
       tank.on(sign_is*v_is,-sign_is*v_is)
       while True:
       if cs.value()<=cs_val_black:
       break
       while True:
       if cs.value()>threshold:
       break

       tank.on(0,0)
       return sign_is

       #--fb--------------------

       def fb(): #3つの缶の上のボールを全てとる動作をまとめた関数
       fb1()
       fb2()
       follow_line(-1)
       intersection(-1,["i","2","r"])
       fb3()


       def fb1(): #Cにあるボールをとる関数
       sign=-1
       reset_arm()
       follow_line_pb(sign,65)
       tank.on_for_degrees(-v_s,-v_s,150)
       turn180(sign)
       set_arm()
       follow_line_fb((-1)*sign,20,0.7)
       fetch_ball()
       tank.on_for_degrees(-v_s,-v_s,110)
       turn180((-1)*sign)
       turn180(sign)

       def fb2(): #Fにあるボールをとる関数
       sign=-1
       reset_arm()
       follow_line_pb(sign,100)
       tank.on_for_degrees(-v_s,-v_s,200)
       turn180(sign)
       set_arm()
       follow_line_fb((-1)*sign,20,0.7)
       fetch_ball()
       tank.on_for_degrees(-v_s,-v_s,0)
       turn90((-1)*sign)

       def fb3(): #Eにあるボールをとる関数
       sign=1
       reset_arm()
       turn180((-1)*sign)
       set_arm()
       follow_line_fb((-1)*sign,20,0.7) #cs sum,ratio
       fetch_ball()
       tank.on_for_degrees(-v_s,-v_s,0)
       turn90((-1)*sign)

       #--pb--------------------
       def pb(): #3つの缶の上全てにボールを置く動作をまとめた関数
       pb1()
       pb2()
       follow_line(-1)
       intersection(-1,["i","2","r"])
       pb3()


       def pb1(): #Cにある缶にボールをおく関数
       sign=-1
       reset_arm()
       follow_line_pb(sign,100)
       down_arm()
       put_ball()
       put_ball()
       reset_arm()
       tank.on_for_degrees(-v_s,-v_s,150)
       turn180(sign)

       def pb2(): #Fにある缶にボールをおく関数
       sign=1
       reset_arm()
       follow_line_pb(sign,100)
       down_arm()
       put_ball()
       put_ball()
       reset_arm()
       tank.on_for_degrees(-v_s,-v_s,200)
       turn90(sign)

       def pb3(): #Eにある缶にボールをおく関数
       sign=-1
       tank.on_for_degrees(-v_s,-v_s,60)
       tank.on_for_degrees(-v_s,v_s,60)
       reset_arm()
       turn180(sign)
       follow_line_pb(sign,100)
       tank.on_for_degrees(-v_s,-v_s,150)
       turn90((-1)*sign)

       #3----follow_line---------------------------------------------------------------------------------

       def follow_line(sign_fl): #交差点までライントレースする関数
       while True:
       #v_r,v_l
       cs_d = max(min(cs.value(),threshold+cs_th_d_max),threshold-cs_th_d_max)-threshold
       if cs_d <= -csd_s_max:
       v_fl=v_b
       k_fl=v_fl*k_b/cs_th_d_max
       elif -csd_s_max < cs_d < csd_s_max:
       v_fl=v_s
       k_fl=v_fl*k_s/cs_th_d_max
       else:
       v_fl=v_w
       k_fl=v_fl*k_w/cs_th_d_max
       v_r_fl = v_fl+sign_fl*k_fl*cs_d
       v_l_fl = v_fl-sign_fl*k_fl*cs_d
       #after_fl
       if cs.value()<cs_val_black:
       after_fl_fl = "list"
       break
       if ts.is_pressed:
       after_fl_fl = "ts_break"
       break
       #move
       tank.on(v_l_fl,v_r_fl)
       tank.off()
       return after_fl_fl

       def follow_line_fb(sign,val_break1,val_break2): #缶の上のボールをアームがとれる位置までライントレースする関数
       while cs2.value(0)+cs2.value(1)+cs2.value(2)<val_break1 or max(cs2.value(0),cs2.value(1),cs2.value(2))/(cs2.value(0)+cs2.value(1)+cs2.value(2)+1)<val_break2: #kann hanntei
       cs_d = max(min(cs.value(),threshold+cs_th_d_max),threshold-cs_th_d_max)-threshold
       if cs_d <= -csd_s_max:
       v_fb=v_b_fb
       k_fb=v_fb*k_b_fb/cs_th_d_max
       elif -csd_s_max < cs_d < csd_s_max:
       v_fb=v_s_fb
       k_fb=v_fb*k_s_fb/cs_th_d_max
       else:
       v_fb=v_w_fb
       k_fb=v_fb*k_w_fb/cs_th_d_max
       v_r_fb = v_fb+sign*k_fb*cs_d
       v_l_fb = v_fb-sign*k_fb*cs_d
       tank.on(v_l_fb,v_r_fb)
       tank.off()

       def follow_line_pb(sign,val_break): #缶の上にボールをのせることができる位置までライントレースする関数
       while us.value()>val_break:
       cs_d = max(min(cs.value(),threshold+cs_th_d_max),threshold-cs_th_d_max)-threshold
       if cs_d <= -csd_s_max:
       v_pb=v_b_pb
       k_pb=v_pb*k_b_pb/cs_th_d_max
       elif -csd_s_max < cs_d < csd_s_max:
       v_pb=v_s_pb
       k_pb=v_pb*k_s_pb/cs_th_d_max
       else:
       v_pb=v_w_pb
       k_pb=v_pb*k_w_pb/cs_th_d_max
       v_r_pb = v_pb+sign*k_pb*cs_d
       v_l_pb = v_pb-sign*k_pb*cs_d
       tank.on(v_l_pb,v_r_pb)
       tank.off()

       #4---u_turn,fb,pb----------------------------------------------------------------------------------
       def reset_arm(): #通信を行いarm_reset()を実行させる
       client = mqtt.Client()
       client.connect(address,1883,60)
       client.publish("arm","reset_arm")
       client.disconnect()
       sleep(3)

       def set_arm(): #通信を行いこの関数とは別のset_arm()を実行させる
       client = mqtt.Client()
       client.connect(address,1883,60)
       client.publish("arm","set_arm")
       client.disconnect()
       sleep(3)

       def fetch_ball(): #通信を行いこの関数とは別のfetch_ball()を実行させる
       client = mqtt.Client()
       client.connect(address,1883,60)
       client.publish("arm","fetch_ball")
       client.disconnect()
       sleep(3)

       def down_arm(): #通信を行いこの関数とは別のdown_arm()を実行させる
       client = mqtt.Client()
       client.connect(address,1883,60)
       client.publish("arm","down_arm")
       client.disconnect()
       sleep(3)

       def put_ball(): #ストックしているボールを一つ送り出す
       sleep(2)
       m_stocker.on_for_degrees(v_stocker,60)
       sleep(2)

       #5-------------------------------------------------------------------------------------
       def main():
       main_break_bool=False
       index_list = 0
       after_fl="after_fl"

       #read file
       #list
       print("start:read list")
       (list,main_break_bool)=read_list("list1")

       #initialize
       if list[0][1] == "r":
       sign = 1
       elif list[0][1] == "l":
       sign = -1
       index_list += 1

       #move
       if not main_break_bool:
       while cs.value()>cs_val_black:
       tank.on(10,10)
       tank.on(0,0)

       while True: #main
       after_fl = follow_line(sign)    #follow_line

       if after_fl == "list":

       if list[index_list]=="end": #end
       tank.on_for_degrees(v_fc,v_fc,270)
       print("list:end")
       break

       else: #list
       if list[index_list][0] == "i":
       print("intersection")
       sign = intersection(sign,list[index_list])
       if list[index_list][0] == "f":
       print("fetch_ball")
       sign = intersection(sign,list[index_list])
       fb()
       if list[index_list][0] == "p":
       print("put_ball")
       sign = intersection(sign,list[index_list])
       pb()
       index_list += 1
       print("index:",index_list)

       elif after_fl == "ts_break": #break
       print("touch sensor break")
       break

       else:
       print("after_fl==after_fl")

       after_fl="after_fl"
       print("")
       else:
       print("main break")
       print("end")
       tank.on(0,0)

       if __name__ == "__main__":
       main()

課題2で使ったプログラムを使っているところもあるので参考にしてください。

反省

製作していく上での計画が上手くいかなかったと感じる。

後期のテストとの被りがありなかなか上手く時間が使えなかった。

面白いロボットを作ろうとしていたが、その割にかけた労力と時間が足りなかった。

時間も製作するうえでとても大切だと感じた。

最後に

今回の課題では本番で上手くいかなかったですが、このゼミを通しては、グループで開発することはとても楽しかった。今後もロボットを作ってみたいという気持ちが強くなりました。


添付ファイル: file14634_2.jpg 1件 [詳細] file14625_2.jpg 1件 [詳細] file2019b-mission2.png 2件 [詳細] file14648_2.jpg 1件 [詳細] file14624_2.jpg 1件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2020-02-14 (金) 15:09:37