#author("2020-01-11T20:38:31+09:00","zhiwen","zhiwen") #author("2020-01-14T17:05:13+09:00","zhiwen","zhiwen") [[2019b/Member]] #contents *課題2 [#e95508c2] 次のように動くロボットの製作 &br;&br; 1人目,2人目,3人目で別々の道を通る課題であるが、ここでは3人目の課題で書く。 +A地点から出発 +J +H (直進) +I (ボール or キューボイドをつかんでそのまま直進) +I' (ボール or キューボイドを離してそのまま直進) +H' (右折) +G' (一時停止の後、直進) +D' (右折) +E', F' 通過 +G' (一時停止の後、直進) +C' (一時停止の後、左折) +B' (一時停止) +A'地点に入る(ゴール) &br; &ref(2019b-mission2.png); *ロボット [#ycbf48fb] 下向きで前の下のほうについてるカラーセンサーによって線に対してどのあたりにいるかを判断する。また、下向きで前の上のほうについてる超音波センサーによってキューボイドがあるかを認識し、キューボイドがある場合は前方の真ん中あたりのモーターにより動く部分によって捕まえる。 &br; &ref(1577342560377_3.jpg); &br; &br; &ref(1577343278032_2.jpg); *プログラム [#nfe0c5f8] #!/usr/bin/env python3 from ev3dev2.motor import Motor,OUTPUT_A,MoveTank,LargeMotor,OUTPUT_B,OUTPUT_C from ev3dev2.sensor.lego import ColorSensor,TouchSensor,UltrasonicSensor from time import sleep m_r = LargeMotor(OUTPUT_C) m_l = LargeMotor(OUTPUT_B) tank = MoveTank(OUTPUT_B,OUTPUT_C) cs = ColorSensor() ts =TouchSensor() us = UltrasonicSensor() fc_m = Motor(OUTPUT_A) threshold = 45 v_s = 12.5 v_c = 5.0 k_s = 0.25 k_c = 2.25 cs_val_black = 15 cs_th_d_max = 20 csd_s_max = 3 v_is = 5 v_fc = 5 v_fc_m = 5 cs.mode ="COL-REFLECT" us.mode ="US-DIST-CM" a1_is=-0.2 a2_is=1 def intersection (sign,index_list_is,list_intersection): print(list_intersection[index_list_is]) n_w = int(list_intersection[index_list_is][0]) if list_intersection[index_list_is][1]=="r": sign_is = 1 elif list_intersection[index_list_is][1]=="l": sign_is = -1 tank.on((a1_is+sign)*v_is,(a1_is-sign)*v_is) print("check1") 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 def fetch_cuboid(sign_fc): fc_m.reset() fc_m.on_to_position(v_fc_m,-90) tank.on_for_degrees(v_fc,-v_fc,180) while cs.value()>cs_val_black: tank.on(v_fc,-v_fc) while cs.value()<threshold: tank.on(sign_fc*v_fc,-sign_fc*v_fc) tank.on(0,0) def fetch_cuboid3(sign_fc): fc_m.reset() fc_m.on_to_position(v_fc_m,-90) tank.on_for_degrees(v_fc,v_fc,600) tank.on_for_degrees(v_fc,-v_fc,180) while cs.value()>cs_val_black: tank.on(v_fc,-v_fc) tank.on(0,0) fc_m.on_to_position(v_fc_m,0) tank.on_for_degrees(-v_fc,-v_fc,60) tank.on_for_degrees(v_fc,-v_fc,180) while cs.value()>cs_val_black: tank.on(v_fc,-v_fc) while cs.value()<threshold: tank.on(sign_fc*v_fc,-sign_fc*v_fc) tank.on(0,0) def follow_line(sign_fl,fc_ab): while True: cs_d = max(min(cs.value(),threshold+cs_th_d_max),threshold-cs_th_d_max)-threshold if -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_c k_fl=v_fl*k_c/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 if fc_ab == "b" and us.value()<110: after_fl_fl = "fc" break if cs.value()<cs_val_black: after_fl_fl = "is" break if ts.is_pressed: after_fl_fl = "ts_break" break tank.on(v_l_fl,v_r_fl) tank.off() return after_fl_fl def main(): main_break_bool=False index_list_is = 0 after_fl="after_fl" fc_ab = "b" #input while True: print("start:num") try: number = int(input("number(1~3):")) if 0<number<4: break except: pass print("number:",number) print("") #read #list_is print("start:list_is") list_intersection=[] file_name = "list_is" + str(number) try: with open(file_name, "r") as f: list_intersection_raw = f.readlines() for i in list_intersection_raw: list_intersection.append(i.strip().split(",")) for n,i in enumerate(list_intersection): list_intersection[n][0] = i[0].strip() list_intersection[n][1] = i[1].strip() if not ((i[0].strip() in {"1","2","3"}) and (i[1].strip() in {"r","l"}) and len(i)==2): print("file content error") raise Exception list_intersection.append("end") print("raw list:",list_intersection_raw) print("list:",list_intersection) print("") except: print("file is_list error") main_break_bool=True #list_sign0 print("start:list_sign0") try: with open("sign0", "r") as f: list_sign0_raw = f.readlines() if list_sign0_raw[number-1].strip()=="r": sign = 1 elif list_sign0_raw[number-1].strip()=="l": sign = -1 else: print("file content error") raise Exception except: print("file sign0 error") main_break_bool=True print("sign0:",sign) print("") #u_turn_rl print("start:u_turn_rl") try: with open("u_turn_rl", "r") as f: list_ut_raw = f.readlines() if list_ut_raw[number-1].strip()=="r": sign_fc = 1 elif list_ut_raw[number-1].strip()=="l": sign_fc = -1 else: print("file content error") raise Exception except: print("file u_turn_rl error") main_break_bool=True print("sign_fc:",sign_fc) print("") #move if not main_break_bool: while cs.value()>cs_val_black: tank.on(10,10) tank.on(0,0) while True: after_fl = follow_line(sign,fc_ab) #follow_line if after_fl == "fc" and fc_ab=="b": #fetch cuboid print("fetch cuboid") if number<3: fetch_cuboid(sign_fc) else: fetch_cuboid3(sign_fc) sign = sign_fc fc_ab = "a" elif after_fl == "is": #intersection if list_intersection[index_list_is]=="end": tank.on_for_degrees(v_fc,v_fc,270) print("list:end") break else: print("intersection") sign = intersection(sign,index_list_is,list_intersection) print("index:",index_list_is) index_list_is += 1 elif after_fl == "ts_break": #touch sensor break print("touch break") break else: print("after_fl error") after_fl="after_fl" else: print("main break") print("end") tank.on(0,0) if __name__ == "__main__": main() *プログラムの説明 [#r223136f] **概要 [#n7177f29] ***動き [#r1a1cda3] 分岐点やカーブ(内側を回る場合)でない場所で線を辿るときはカラーセンサーが線の黒と白の境界(どのあたりが境界かはプログラム内で指定する)にくるようにタイヤの速度を調整しながら線をたどっていく。 プログラム開始直後はAからラインまで進み、そこで時計回りか反時計回り(読み込むファイル内で指定する)にその周辺を回り指定された道を進み、そのあとは線をたどりながら分岐点や内側の境界で曲がるカーブが来たらスタート時と同様に分岐点付近を回り指定された道をとり、ゴールまで向かう。 ***準備・動かし方 [#q91e54e0] 以下に書いてあるファイル:list_is,sign0 ,u_turn_rlを用意し、プログラムを実行し、何人目の道を辿るかを表す数字を入力する。 **定数(プログラム実行中や与えるデータによって値が変化しない変数) [#kb01b44b] m_r = LargeMotor(OUTPUT_C) m_l = LargeMotor(OUTPUT_B) tank = MoveTank(OUTPUT_B,OUTPUT_C) cs = ColorSensor() ts =TouchSensor() us = UltrasonicSensor() fc_m = Motor(OUTPUT_A) cs.mode ="COL-REFLECT" us.mode ="US-DIST-CM" threshold = 45 値 :境界として使う場所でのcsの値 使ってる場所 :intersection(),fetch_cuboid(),fetch_cuboid3(),follow_line() v_s = 12.5 値 :まっすぐ進むときのtankの速さ 使ってる場所 :follow_line() v_c = 5.0 値 :曲がるときのtankの速さ 使ってる場所 :follow_line() k_s = 0.25 値 :まっすぐ進むときのtankの曲がりやすさ 使ってる場所 :follow_line() k_c = 2.25 値 :曲がるときのtankの曲がりやすさ 使ってる場所 :follow_line() cs_val_black = 15 値 :黒いと判断させたい場所でのcsの値の中での最大値 使ってる場所 :intersection(),fetch_cuboid(),fetch_cuboid3(),follow_line() cs_th_d_max = 20 値 :csの値を使うときその値を-cs_th_d_max<=csの値<=cs_th_d_maxの範囲で制限 するために使う値 使ってる場所 :follow_line() csd_s_max = 3 値 :-csd_s_max<threshold-csの値<csd_s_maxのときまっすぐ進んでると判断させ るために使う値 使ってる場所 :follow_line() v_is = 5 値 :intersection()実行時のtankの速さ 使ってる場所 :intersection() v_fc = 5 値 :fetch_cuboid()またはfetch_cuboid3()実行時のtankの速さ 使ってる場所 :fetch_cuboid(),fetch_cuboid3() v_fc_m = 5 値 :fetch_cuboid()またはfetch_cuboid3()実行時のfc_mの速さ 使ってる場所 :fetch_cuboid(),fetch_cuboid3() a1_is=-0.2 値 :基本的には0以下で-1より大きく、intersection()実行時にtankを分岐点から 遠ざかる方向に外側に向くように回転させるときにどのくらい大きく回転さ せるかを決める値 使ってる場所 :intersection() a2_is=1 値 :基本的には0より大きく、intersection()実行時にtankが道を探すため分岐点 周辺を回るときにどのくらい大きく回るかを決める値 使ってる場所 :intersection() **変数(プログラム実行中や与えるデータによって値が変化する変数) [#s9d4b8d9] 読み込みはmain()に含まれているが、ここではmain()と書いた場合はmain()関数のうち読むこむ部分を除いたものとする。 &br; fetch_cuboid()と書いたときはfetch_cuboid3()も含むとする。 &br; 使っている場所に書いてるものは関数実行時に引数として渡している関数も含む。 ***main_break_bool [#jedc8414] main_break_bool とりうる値 :True,False どんな値か :main()でファイルを読み込みそこでエラーやファイルの書き方に間違いが あったときTrueとなり、main()の動かす部分を行わずにプログラムを終了 する。 使っている場所 :読み込み,main() ***index_list_is [#yadc0a62] index_list_is とりうる値 :0,1,2,... どんな値か :最初は0でありintersection()が行われるたびに1増え、intersection()実 行時、list_intersection[index_list_is]を使う。 使っている場所 :intersection(),main() ***after_fl [#maaf36df] after_fl とりうる値 :"after_fl","fc","is","ts_break" どんな値か :follow_line()実行時に交差点に差し掛かったりcuboidが見つかるなどによ りfollow()を終了するが、そのあと何をするかを表す値 "fc" :fetch_cuboid()を実行する "is" :intersection()を実行する "ts_break":タッチセンサーが押されたときにこの値になり、main()を終わ らせる "after_fl":初期化のときに使い、follow_line()実行時に代入はされず、 特に意味のある値ではない。 使っている場所 :intersection(),読み込み ***fc_ab [#i4a4a770] fc_ab とりうる値 :"a","b" どんな値か :最初は"b"でありfetch_cuboid()が実行されると"a"になる値で、 fetch_cuboid()が複数回実行されるのを防ぐために使われている。 使っている場所 :fetch_cuboid(),follow_line(),main() ***number [#gddbe997] number とりうる値 :1,2,3 どんな値か :プログラム実行開始時に入力する値でlist_isの何番のファイルを読み込む か、その他の読み込むファイルの何行目を使うかを決める値 使っている場所 :main(),読み込み ***list_intersection [#kc00bd0c] list_intersection とる値の例 :[[2,l],[2,l],[1,l],[1,l],[1,r],[1,l],"end"] どんな値か :何回目の分岐(分岐と判断される場所)でどのような道をとるかを決める値 でファイルlist_isから読み込む("end"はファイルには書かないもので自動 で追加される) [数字n,文字a]:進行方向に合わせて時計回りか反時計回りで近いほうから n番目の道でa=rなら右、a=lなら左の境界をとる。 "end" :ゴールに着いたこと表す値 使っている場所 :intersection(),main(),読み込み ***sign [#a8ede537] sign とりうる値 :-1,1 どんな値か :follow_line()実行時右と左のどっちの境界をとっているかを表す値で、 右の時1、左の時-1をとる。 使っている場所 :intersection(),follow_line(),main(),読み込み ***sign_fc [#iddbc59b] sign_fc とりうる値 :-1,1 どんな値か :(),fetch_cuboid3()の実行後に右と左のどちらの境界をとる かを表す値で、右の時1、左の時-1をとる。ファイルu_turn_rlから読み込 む。 使っている場所 :fetch_cuboid(),main(),読み込み **関数 [#s5d875de] ***intersection() [#de5621fa] 引数について sign とる値 :1,-1 どんな値か :intersection()実行前にとってる境界が右か左か index_list_is とる値 :整数 どんな値か :何回目の分岐かを表す値 list_intersection とる値 :二次元リスト どんな値か :何回目の分岐でなにをするかを表すリスト 戻り値について sign_is とる値 :1,-1 どんな値か :intersectiont()実行後にとる境界が右か左かを表す値 内容 def intersection (sign,index_list_is,list_intersection): print(list_intersection[index_list_is]) """道の番号とその道でとる境界の右左の表 示""" n_w = int(list_intersection[index_list_is][0]) """道の番号""" if list_intersection[index_list_is][1]=="r": """とる境界の右左に合わせてsign_isのを 1か-1に決める""" sign_is = 1 elif list_intersection[index_list_is][1]=="l": sign_is = -1 tank.on((a1_is+sign)*v_is,(a1_is-sign)*v_is) """tankを分岐点から遠ざかる方向に外 側に向くように回転させる""" print("check1") 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が 行くように調整してそこで止める""" 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 ***fetch_cuboid() [#n7dab819] 引数について sign_fc とる値 :1,-1 どんな値か :fetch_cuboid()が終わった後でとる境界が右か左かを表す値 内容 def fetch_cuboid(sign_fc): fc_m.reset() """fc_mの角度をリセットする""" fc_m.on_to_position(v_fc_m,-90) """cuboidを捕まえる""" tank.on_for_degrees(v_fc,-v_fc,180) """tankの向きをおよそ90度回転させる""" while cs.value()>cs_val_black: """tank.on_for_degrees(v_fc,-v_fc,180)と同じ向きで 黒いラインまで回転する""" tank.on(v_fc,-v_fc) while cs.value()<threshold: """黒いラインの中からsign_fcに合わせて右か左の境界まで 移動して止める""" tank.on(sign_fc*v_fc,-sign_fc*v_fc) tank.on(0,0) ***fetch_cuboid3() [#be2b1a85] 引数について sign_fc とる値 :1,-1 どんな値か :fetch_cuboid3()が終わった後でとる境界が右か左かを表す値 内容 def fetch_cuboid3(sign_fc): fc_m.reset() """fc_mの角度をリセットする""" fc_m.on_to_position(v_fc_m,-90) """cuboidを捕まえる""" tank.on_for_degrees(v_fc,v_fc,600) """tankを前に進める""" tank.on_for_degrees(v_fc,-v_fc,180) """tankの向きをおよそ90度回転させる""" while cs.value()>cs_val_black: """tank.on_for_degrees(v_fc,-v_fc,180)と同じ向きで黒いラインまで回転して止める""" tank.on(v_fc,-v_fc) tank.on(0,0) fc_m.on_to_position(v_fc_m,0) """cuboidを放す""" tank.on_for_degrees(-v_fc,-v_fc,60) """tankを少し後ろに下げる""" tank.on_for_degrees(v_fc,-v_fc,180) """tankの向きをおよそ90度回転させる""" while cs.value()>cs_val_black: """tank.on_for_degrees(v_fc,-v_fc,180)と同じ向きで黒いラインまで回転する""" tank.on(v_fc,-v_fc) while cs.value()<threshold: """黒いラインの中からsign_fcに合わせて右か左の境界まで移動して止める""" tank.on(sign_fc*v_fc,-sign_fc*v_fc) tank.on(0,0) ***follow_line() [#cec601d2] 引数について sign_fl とる値 :1,-1 どんな値か :follow_line()する境界が右か左かを表す値 fc_ab とる値 :"a","b" どんな値か :fetch_cuboid()またはfetch_cuboid3()がすでに実行されたかを表す値 戻り値について after_fl_fl とる値 :"fc","is","ts_break" どんな値か :follow_line()実行後になにをするかを表す値 内容 def follow_line(sign_fl,fc_ab): while True: cs_d = max(min(cs.value(),threshold+cs_th_d_max),threshold-cs_th_d_max)-threshold """csでとった値と境界での値の差を ある範囲で制限したもの""" if -csd_s_max < cs_d < csd_s_max: """tankの速さを決める(まっすぐ進むときと曲が るときで変えてある)""" v_fl=v_s k_fl=v_fl*k_s/cs_th_d_max else: v_fl=v_c k_fl=v_fl*k_c/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 if fc_ab == "b" and us.value()<110: """ある条件を満たすときfollow_line()を止め て、その条件に応じてそのあと行動が決まる ので、その行動に対応してる文字列を after_fl_flに代入する""" after_fl_fl = "fc" break if cs.value()<cs_val_black: after_fl_fl = "is" break if ts.is_pressed: after_fl_fl = "ts_break" break tank.on(v_l_fl,v_r_fl) """v_l_flとv_r_flを使ってtankを動かす""" tank.off() return after_fl_fl ***main() [#vdea6671] def main(): main_break_bool=False """ファイル読み込みでエラーが起きたときTrueになり、そうでない ときFalseをとるboolean型の変数""" index_list_is = 0 """何番目の分岐点か""" after_fl="after_fl" """:follow_line()が止まってから何をするかを保存しておく変数""" fc_ab = "b" """aかb:fetch_cuboid()またはfetch_cuboid3()がすでに実行されたか (a:すでに実行された,b:まだ実行されてない)""" #input while True: """1か2か3を入力し、それをnumberに代入する""" print("start:num") try: number = int(input("number(1~3):")) if 0<number<4: break except: pass print("number:",number) print("") #read -------------------------------------------------------------------------------------- 読み込み -------------------------------------------------------------------------------------- #move if not main_break_bool: """ファイル読み込みでエラー等がなければこの中身を実行する""" while cs.value()>cs_val_black: """ラインに着くまでまっすぐ進む""" tank.on(10,10) tank.on(0,0) while True: """このwhileの中がメインのプログラム""" after_fl = follow_line(sign,fc_ab) """follow_line()を実行"""#follow_line if after_fl == "fc" and fc_ab=="b": """usによってキューボイドがあると判断さ れプログラムを始めてからまだ fetch_cuboid()またはfetch_cuboid3() が一度も実行されてなければnumberに応 じてそれらのうちいずれかを実行す る""" #fetch cuboid print("fetch cuboid") if number<3: fetch_cuboid(sign_fc) else: fetch_cuboid3(sign_fc) sign = sign_fc fc_ab = "a" elif after_fl == "is": """list_intersection[index_list_is]がendならばtankを 少し進めてから止めてメインのプログラムを止め,そう でなければlist_intersection[index_list_is]に合わ せてintersection()を実行する""" #intersection if list_intersection[index_list_is]=="end": tank.on_for_degrees(v_fc,v_fc,270) print("list:end") break else: print("intersection") sign = intersection(sign,index_list_is,list_intersection) print("index:",index_list_is) index_list_is += 1 elif after_fl == "ts_break": """tsが押されたら止める"""#touch sensor break print("touch break") break else: """after_flに正しい値が入力されていることの確認用(正しい値が代入され てなければこれの中身を実行する)""" print("after_fl error") after_fl="after_fl" else: print("main break") print("end") tank.on(0,0) if __name__ == "__main__": main() **読み込み [#t5b8e99c] ***list_is [#i4f2528e] #list_is print("start:list_is") """list_is(n)という名前のファイルを読み込みlist_intersection という名前の二次元リストでまとめる。(n)はプログラム実行時 に入力する数字とする""" list_intersection=[] file_name = "list_is" + str(number) try: with open(file_name, "r") as f: list_intersection_raw = f.readlines() for i in list_intersection_raw: list_intersection.append(i.strip().split(",")) for n,i in enumerate(list_intersection): list_intersection[n][0] = i[0].strip() list_intersection[n][1] = i[1].strip() if not ((i[0].strip() in {"1","2","3"}) and (i[1].strip() in {"r","l"}) and len(i)==2): print("file content error") raise Exception list_intersection.append("end") print("raw list:",list_intersection_raw) print("list:",list_intersection) print("") except: print("file is_list error") main_break_bool=True ***sign0 [#w7280591] #list_sign0 print("start:list_sign0") """sign0という名前のファイルを読み込みnumber行目の値に応 じてsignに1か-1を代入する""" try: with open("sign0", "r") as f: list_sign0_raw = f.readlines() if list_sign0_raw[number-1].strip()=="r": sign = 1 elif list_sign0_raw[number-1].strip()=="l": sign = -1 else: print("file content error") raise Exception except: print("file sign0 error") main_break_bool=True print("sign0:",sign) print("") ***u_turn_rl [#rddc2e62] #u_turn_rl print("start:u_turn_rl") """u_turn_rlという名前のファイルを読み込み(n)行目の値に応 じてsign_fcに1か-1を代入する。(n)はプログラム実行時に入 力する数字とする""" try: with open("u_turn_rl", "r") as f: list_ut_raw = f.readlines() if list_ut_raw[number-1].strip()=="r": sign_fc = 1 elif list_ut_raw[number-1].strip()=="l": sign_fc = -1 else: print("file content error") raise Exception except: print("file u_turn_rl error") main_break_bool=True print("sign_fc:",sign_fc) print("") **読み込むファイル [#ddf72ff5] 次のようなファイルプログラム実行前に用意する必要がある。 ***list_is(list_is3) [#sa8e8c0f] list_isは自分がライントレースする道が何人目の課題かによってlist_isの直後にその数字を 付け加えたものをファイル名として保存する。今回は3人目の課題について書くのでファイル名はlist_is3とする。 2,r 1,l 2,r 1,r 2,r 1,l 2,l 1,r ***sign0 [#t3686132] r r r ***u_turn_rl [#jbd78aed] l r r *結果と感想 [#n92e680c] **結果 [#w530b5e2] 発表ではプログラム自体はできていたが、細かいデバッグや定数の調整などが不十分であり、一人目の課題ではゴールまではたどり着けなかったが二人目の課題ではうまく最後までライントレースができた。発表後、さらに調整を行うことにより全てのコースでライントレースすることができ、速度も少し上げることができた。 **感想 [#df2f80e9] ハード面の工夫が苦手であり、ミッション1ではそれにより苦戦した部分があったと思うが、今回はそこをグループのメンバーに補ってもらい、ミッション1より完成度の高いものが作ることができた。ミッション3でもグループとして協力できるように計画的に進めたり積極的に意見を交換するなどの努力をしていきたいと思う。