![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
#author("2020-01-06T18:09:16+09:00","LaiHen","LaiHen") #author("2020-01-06T18:53:26+09:00","LaiHen","LaiHen") ---- 目次 #contents ---- * 課題２：ライントレース [#u7470bfe] 下の画像の指定されたコースを黒線に沿って進む &ref(2019b/Member/LaiHen/Mission2/2019b-mission2.png,70%,コースの全体図); ** 指定コース [#vce05226] + A地点から出発 + B + C (直進) + D (一時停止の後、直進) + E, F 通過 + G (一時停止の後、左折) + H (一時停止の後、左折) + I (キューボイドをつかんでUターン) + H (直進) + J (一時停止) + A地点に入る(ゴール) * ロボットについて [#h73f63eb] ** ロボットの構造 [#d613c006] 今回の課題でロボットに必要な物は本体を移動させるためのモーター２つ以外に、黒線に沿って進むために色を感知するカラーセンサー、キューボイドとの距離を測る超音波センサー、キューボイドをつかむモーターで上げ下げできるアームの3つです。これらをうまく使うためにそれぞれに工夫した点があります。 *** カラーセンサー [#h529f137] カラーセンサーは今回のライントレースで最も重要な部分です。しかも本体の移動はこのセンサーを用いて決定するためタイヤとの位置も考えなくてはいけません。今回のコースで最も難しいと思われるのはHとJの間にある半径5ｃｍの半円部分です。もしセンサーが本体の軸から５ｃｍ以上離れている場合、本体をバックさせないとまわれません。そのため５ｃｍを超えないようにセンサーを設置しました。 *** 超音波センサー [#c9e9e386] 前提としてキューボイドとの測る際本体はライントレースをしている関係上線の左右どちらかに寄っています。そのため本体の中央に設置してもセンサーの正面にキューボイドは来ません。そのためセンサーを左寄りに設置し、キューボイドをつかむ際は線の右側をライントレースするとセンサーの中央にキューボイドが来るようにしました。 *** アーム [#b9c5da66] アームの設置位置は超音波センサーを基準にしたので簡単でしたが、2つのセンサーを付けたためロボット自体が縦に長くなってしまったのでできるだけアームはコンパクトになるようにしました。 ** 全体的な工夫点 [#x6509f6c] EV3のセンサーは精度が高いため振れなどによって値が変化してしまいます。ですがロボットの画像からわかるように縦に長く動くたびに揺れてしまいます。それを防ぐためにできるだけ固定をし、前方への倒れこみを軽減するために2枚目の画像のようにタイヤをつけました。 &ref(2019b/Member/LaiHen/Mission2/KD22.jpg,25%,ロボットの全体画像); &ref(2019b/Member/LaiHen/Mission2/KD21.jpg,25%,補助輪画像); &ref(2019b/Member/LaiHen/Mission2/KD24.jpg,25%,センサー関係の画像); ** ロボットについてのまとめ [#c64c38da] 前回はあまりロボットにかかわらなかったのでいろいろと試行錯誤してロボットを作成しました。ですが安定させることを重視するあまりかなり大きくなってしまいました。課題の内容から考えるとできるだけコンパクトの方が望ましいと思うので、次回は課題を達成するだけでなくその課題にふさわしいロボットを作成しようと思います。 * プログラムについて [#f9aa9a86] ** プログラムのシステムについて [#i0b559db] ライントレースは黒線の上を進むのではなく黒と白の境界部分を進んでいく方法で行います。また、今回はロボットの作業量が多いため動きの調整を簡単にできるようにいくつかの数値を入力すれば後はすべて計算をし、値を決定していく仕組みにします。加えて、動作ごとに関数を作成し後から動きの指示をしやすくします。これらをもとに作成したのが以下のようなプログラムです。 #!/usr/bin/env python3 from ev3dev2.motor import * from ev3dev2.sensor import * from ev3dev2.sensor.lego import * from time import * Wmax,Bmax,spmax,spmin,brockch,tst=75,5,8,4,6,1.5 tw = MoveTank(OUTPUT_B, OUTPUT_C) Mm= MediumMotor(OUTPUT_A) cs,ts,us = ColorSensor(),TouchSensor(),UltrasonicSensor() us.mode = 'US-DIST-CM' Mv=(Wmax+Bmax)/2 Lp=(Wmax-Mv)/4 llp=2*Lp spab=(spmax+spmin)/2 Clev=[Wmax,Wmax-Lp,Mv+Lp,Mv-Lp,Bmax+Lp,Bmax] class Linetorace: def __init__(self,a): self.cv=a self.colortyp,self.dcv,self.lesp,self.risp=0,0,0,0 self.typcheck() self.movesp() def typcheck(self): for i in range(5): if Clev[i+1]<=self.cv<Clev[i]: self.colortyp=i self.dcv=Clev[i]-self.cv def movesp(self): zuw=round(self.dcv/llp,1) if self.colortyp==0: self.lesp=spab-(spab-spmin)*zuw self.risp=-spmin elif self.colortyp==1: self.lesp=spmax self.risp=spab+(spmax-spab)*zuw elif self.colortyp==2: self.lesp=spmax self.risp=spmax elif self.colortyp==3: self.lesp=spab+(spmax-spab)*zuw self.risp=spmax else: self.lesp=-spmin self.risp=spab-(spab-spmin)*zuw class root: def __init__(self,v1,v2): self.typ,self.chst=v1,v2 def action(self): if self.typ==0: follow_line(self.chst,50) elif self.typ==1: closs(self.chst) elif self.typ==2: fetch_cuboid(self.chst) else: gostart() def follow_line(chst,ched):#ライントレース用プログラム sttime1,notime1 = time(),time() bcon=0 while not (ts.is_pressed) and not ((notime1-sttime1)>ched): cvp = cs.reflected_light_intensity MLT=Linetorace(cvp) if MLT.colortyp==4: bcon=bcon+1 else: bcon=0 notime1=time() if (chst <= (notime1-sttime1) <=ched) and (bcon== brockch): break tw.on(MLT.lesp,MLT.risp) sleep(0.1) tw.off() def closs(pp):#交差点と直角カーブの際の行動用プログラム cvp= cs.reflected_light_intensity MLT = Linetorace(cvp) tw.on(-MLT.lesp,-MLT.risp) sleep(0.1*brockch) tw.off() if pp==0: follow_line(tst,tst+0.1) else: sleep(5) if pp==1: follow_line(tst,tst+0.1) elif pp==2: tw.on_for_seconds(spmax,spmax,1) else: sttime=time() notime=time() while (notime- sttime <=tst): cvp= cs.reflected_light_intensity MLT= Linetorace(cvp) tw.on(MLT.risp,MLT.lesp) sleep(0.1) notime=time() tw.off() def fetch_cuboid(edti):#ライントレース後キューブを回収しUターン、その後ライントレース再開 sttime2 = time() fase=0 while not (ts.is_pressed): cvp= cs.reflected_light_intensity MLT= Linetorace(cvp) tw.on(MLT.risp,MLT.lesp) notime2= time() txle= us.distance_centimeters if txle <= 4 : tw.on(0,0) Mm.on_for_seconds(-10,1.2) tw.on_for_degrees(-spmax,spmax,360) fase=1 break if (notime2-sttime2) >= edti: fase=1 break sleep(0.1) if fase==1: follow_line(5,edti) tw.off() Mm.off() def gostart():#スタート時の前進用プログラム tw.on_for_seconds(spmax,spmax,2) def main(): MAP=[root(4,0),root(0,0),root(1,0),root(0,0),root(1,1),root(0,0),root(1,1),root(2,30),root(0,20),root(1,2)] for i in range(len(MAP)): if not ts.is_pressed: MAP[i].action() else: break print("all clear") if __name__ == '__main__': main() このプログラムについて以下で8つに分けて詳しく説明していきます *** �．廛蹈哀薀爐了�前設定と基準値の算出 [#xacf6790] #!/usr/bin/env python3 from ev3dev2.motor import * from ev3dev2.sensor import * from ev3dev2.sensor.lego import * from time import * この部分で必要な物のインポートをする Wmax,Bmax,spmax,spmin,brockch,tst=75,5,8,4,6,1.5 ここで使う数値を入力します。この部分以外では極力数値を直接指定しないでおくことでプログラム全体の調整を簡単にすることができる。 入力するのは + 白色のセンサーの値(Wmax) ==75 + 黒色のセンサーの値(Bmax) ==5 + モーターの最高速度(spmax) ==8 + モーターの最低速度(spmin) ==4 + 交差点判断する黒色のカウント値(brockch) ==6 + 交差点等のコマンドの継続時間(tst) ==1.5~ 上記の6つです。 これ以外の数値はこれらをもとに計算していきます - モーターやセンサーを使うための準備を行います。 tw = MoveTank(OUTPUT_B, OUTPUT_C) Mm= MediumMotor(OUTPUT_A) cs,ts,us = ColorSensor(),TouchSensor(),UltrasonicSensor() us.mode = 'US-DIST-CM' - 入力された数値からプログラムで使うものの決定をしていきます。 Mv=(Wmax+Bmax)/2 Lp=(Wmax-Mv)/4 llp=2*Lp spab=(spmax+spmin)/2 Clev=[Wmax,Wmax-Lp,Mv+Lp,Mv-Lp,Bmax+Lp,Bmax] -- 白色と黒色の境界線（灰色）でのセンサーの数値 灰色の値を計測する場合正確に境界線の中央を測ることは難しいため、細かくロボットを動かしずらくなってしまう。そのため白色と黒色の値の平均値を灰色の値にした。 Mv=(Wmax+Bmax)/2 -- 色の段階を判断の範囲を決定する 白から黒までを均等に分けるのに使う。ここでは（白色の値-灰色の値）/4とする。 Lp=(Wmax-Mv)/4 llp=2*Lp -- 速度の平均値を決定 ライントレースの速度の計算の際に最高速度と最低速度の平均値を使うため先に計算しておく spab=(spmax+spmin)/2 -- 色の範囲の基準値をリストとして決定する 白から黒までを5段階に分けるときの基準値を先に求めた数値から決める 各段階の基準値は + Wmax〜Wmax-Lp + Wmax-Lp〜Mv+Lp + Mv+Lp〜Mv-Lp + Mv-Lp〜Bmax+Lp + Bmax+Lp〜Bmax Clev=[Wmax,Wmax-Lp,Mv+Lp,Mv-Lp,Bmax+Lp,Bmax] - これで今後使う変数の準備ができました。プログラムを起動してすぐに変数を決めるようにしたので関数内などで毎回計算する必要がないので各関数の処理がわずかですが速くすることができます。 *** �▲薀ぅ鵐肇譟璽考僂離�ラス [#dc918a51] class Linetorace: def __init__(self,a): self.cv=a self.colortyp,self.dcv,self.lesp,self.risp=0,0,0,0 self.typcheck() self.movesp() def typcheck(self): for i in range(5): if Clev[i+1]<=self.cv<Clev[i]: self.colortyp=i self.dcv=Clev[i]-self.cv def movesp(self): zuw=round(self.dcv/llp,1) if self.colortyp==0: self.lesp=spab-(spab-spmin)*zuw self.risp=-spmin elif self.colortyp==1: self.lesp=spmax self.risp=spab+(spmax-spab)*zuw elif self.colortyp==2: self.lesp=spmax self.risp=spmax elif self.colortyp==3: self.lesp=spab+(spmax-spab)*zuw self.risp=spmax else: self.lesp=-spmin self.risp=spab-(spab-spmin)*zuw 引数として送られてきたカラーセンサーの値を5段階のうちどれに該当するかを判定する。その後それぞれの段階ごとに左右のモーターの速度を決める。 カラーセンサーの値を5段階のうちどれに該当するかを判定する。その後それぞれの段階ごとに左右のモーターの速度を決める。~ このクラスはセンサーの値から左右の速度、段階などを取得できるのでカラーセンサー関係を一括で管理することができます。~ このクラスのメインである段階分けと左右の速度決定を説明していきます。 + 段階分け カラーセンサーの値を以下の表と照らし合わせて段階数を決めます。ここでの範囲は灰色を基準に対称になるようになっています。 |色の段階|白色|白色と灰色の中間|灰色|黒色と灰色の中間|黒色| |段階数|0|1|2|3|4| |値の範囲|Wmax〜Wmax-Lp|Wmax-Lp〜Mv+Lp|Mv+Lp〜Mv-Lp|Mv-Lp〜Bmax+Lp|Bmax+Lp〜Bmax| + 左右の速度決定 段階数が決まったらその段階数ごとに決められた速度を求める計算式にセンサーの値を代入していきます。ここでの基準は黒線の左側としています。~ ''（決定した段階の上限-カラーセンサーの値）÷（範囲の幅×２）''の値を''調整値''とする。 |段階数|0|1|2|3|4| |右の速度(文字)|-（最低速度）|速度の平均値+（最高速度-速度の平均値）×調整値|最高速度|最高速度|速度の平均値-（速度の平均値-最低速度）×調整値| |右の速度(式)|-spmin|spab+(spmax-spab)*zuw|spmax|spmax|spab-(spab-spmin)*zuw| |左の速度(文字)|速度の平均値-（速度の平均値-最低速度）×調整値|最高速度|最高速度|速度の平均値+（最高速度-速度の平均値）×調整値|-（最低速度）| |左の速度(式)|spab-(spab-spmin)*zuw|spmax|spmax|spab+(spmax-spab)*zuw|-spmin| 表からわかるように段階が0と4、1と3は式が左右反対だけで計算式は同じにしてあります。 - ''任意による左右の基準を変更可能''~ 段階分けの対称および計算式の調整によってライントレースの基準を左右のどちらかでもこのクラス1つで速度を求めることができるのです。~ -- 左側の時は右の速度を右のモーターに左の速度を左のモーターに送る -- 右側の時は右の速度を左のモーターに左の速度を右のモーターに送る *** ��ルート指定用のクラス [#o79a6f60] class root: def __init__(self,v1,v2): self.typ,self.chst=v1,v2 def action(self): if self.typ==0: follow_line(self.chst,50) elif self.typ==1: closs(self.chst) elif self.typ==2: fetch_cuboid(self.chst) else: gostart() main部分でルートのコードを打ち込んでいくのは大変なので2つの引数でルートを指定できるようにした。 *** �つ名鏤�のライントレース用の関数 [#z4db0b7e] def follow_line(chst,ched):#ライントレース用プログラム sttime1,notime1 = time(),time() bcon=0 while not (ts.is_pressed) and not ((notime1-sttime1)>ched): cvp = cs.reflected_light_intensity MLT=Linetorace(cvp) if MLT.colortyp==4: bcon=bcon+1 else: bcon=0 notime1=time() if (chst <= (notime1-sttime1) <=ched) and (bcon== brockch): break tw.on(MLT.lesp,MLT.risp) sleep(0.1) tw.off() 2つの引数（交差点判定開始までの時間、関数の処理の制限時間）を使う関数です。この関数内で行う処理は大きく分けると - 1つ目は今のカラーセンサーの値から左右のモーターの速度を取得しそれをそれぞれのモーターに送る - 2つ目は交差点または直角カーブに来ているかどうか の２つです。これらの処理について説明していきます。 + ライントレース ここでの処理はタッチセンサーが押されておらずまた、関数の処理を実行してからの時間が制限時間を超えていないときに、今のカラーセンサーの値を用意してあるライントレース用のクラスに代入しモーターの速度を求める。その後その値を左右のモーターに送る。ここでのライントレースはクラスでの決めた左右をそのまま使うため黒線の左側を基準としています。 while not (ts.is_pressed) and not ((notime1-sttime1)>ched): cvp = cs.reflected_light_intensity MLT=Linetorace(cvp) tw.on(MLT.lesp,MLT.risp) sleep(0.1) + 交差点判定 交差点判定は最初はカラーセンサーの変化の具合から判定しようと試みましたがロボットを動かす環境の状況によってセンサーの変化に影響がありました。そのため今回は5段階での動きから判定することにしました。~ ここでの処理は1つ目の処理で使ったクラスの段階が黒色の場合のみカウントし、そのカウント数が先に決めた黒色判定までのカウント数になったら現在いるのは交差点と断定し、この関数の処理を停止し、左右のモーターを停止させます。 if MLT.colortyp==4: bcon=bcon+1 else: bcon=0 notime1=time() if (chst <= (notime1-sttime1) <=ched) and (bcon== brockch): break この関数は0.1秒おきに色のチェックをするため曲線部分での交差点判定のミスがありました。なので、カウントするのは黒色の時だけにし、もしそれ以外の段階が来たらカウントをリセットする仕組みすることで判定のミスを抑えました。 *** �ツ廠僖�ーブと交差点での行動用関数 [#ca2a43c6] def closs(pp):#交差点と直角カーブの際の行動用プログラム cvp= cs.reflected_light_intensity MLT = Linetorace(cvp) tw.on(-MLT.lesp,-MLT.risp) sleep(0.1*brockch) tw.off() if pp==0: follow_line(tst,tst+0.1) else: sleep(5) if pp==1: follow_line(tst,tst+0.1) elif pp==2: tw.on_for_seconds(spmax,spmax,1) else: sttime=time() notime=time() while (notime- sttime <=tst): cvp= cs.reflected_light_intensity MLT= Linetorace(cvp) tw.on(MLT.risp,MLT.lesp) sleep(0.1) notime=time() tw.off() 引数でカーブ、交差点を左折、右折、直進の4つを指示しそれぞれの処理を実行する関数です。ここでの処理は + 現在のカラーセンサーの値からさっきまでの道を戻る + その後4つそれぞれの動きを実行する といった流れです。~ なぜ交差点での処理で一旦道を戻るのはこの関数が呼び出された時のロボットの状況にあります。この関数を呼ぶということは交差点にいるということです。この時ロボットはライントレースで黒色の判定なので左に曲がっています。次の処理がカーブや左折であれば問題ありませんが直進や右折の場合は動きが複雑にあってしまいます。これを簡単にするために交差点判定のカウントを始める直前のロボットがおおむね黒線に対して平行の状態まで道を戻る処理を行います。~ 後は引数の値で処理を変えて4つの行動を分けます。 ++ カーブ~ カーブの場合は一旦止まる必要はないのでそのまま黒線の左側を基準にライントレースを行います。 ++ 交差点を左折~ 交差点なので一定時間停止後カーブと同様の処理をします。 ++ 交差点を直進~ 交差点なので一定時間停止後左右のモーターを最大速度で1秒間動かします。 ++ 交差点を右折~ 交差点なので一定時間停止後黒線の右側を基準にライントレースを行います。 *** �Ε�ューブキャッチ用関数 [#s7a772b7] def fetch_cuboid(edti):#ライントレース後キューブを回収しUターン、その後ライントレース再開 sttime2 = time() fase=0 while not (ts.is_pressed): cvp= cs.reflected_light_intensity MLT= Linetorace(cvp) tw.on(MLT.risp,MLT.lesp) notime2= time() txle= us.distance_centimeters if txle <= 4 : tw.on(0,0) Mm.on_for_seconds(-10,1.2) tw.on_for_degrees(-spmax,spmax,360) fase=1 break if (notime2-sttime2) >= edti: fase=1 break sleep(0.1) if fase==1: follow_line(5,edti) tw.off() Mm.off() キューボイドをキャッチし、Uターンして戻る処理を行う関数です。この関数の処理とその説明をしていきます。 + キューボイドまでライントレース~ ロボットについての説明で述べたように超音波センサーはライントレースを考慮して左側についているのでそれがキューボイドの正面に来る黒線の右側を基準にライントレースライントレースしていきます。 cvp= cs.reflected_light_intensity MLT= Linetorace(cvp) tw.on(MLT.risp,MLT.lesp) notime2= time() txle= us.distance_centimeters + キューボイドの検知とキャッチ~ 超音波センサーの値が決められた数値以下になったらミディアムモーターを動かしてキャッチします。最初はこの基準値も�，良�分で入力する予定でしたがロボットが縦長でキューボイドの探索を実行するときにはすぐ目の前に来てしまうのでなくしました。 if txle <= 4 : tw.on(0,0) Mm.on_for_seconds(-10,1.2) + Uターン~ キューボイドをキャッチ後その場で反時計回りに半回転します tw.on_for_degrees(-spmax,spmax,360) + 来た道を戻る~ Uターンして先ほどまで通ってきた線に戻ってきます。ですが先ほどは黒線の右側でしたがUターンすると右側だったのが左側になるのでライントレースも左側基準に変更します。 if fase==1: follow_line(5,edti) *** �Д好拭璽藩僂隆愎� [#q7516c13] def gostart():#スタート時の前進用プログラム tw.on_for_seconds(spmax,spmax,2) AからBに進むときライントレースで進むのは大変のなのでただ直進させる関数 *** �┘瓮ぅ鵐廛蹈哀薀� [#y525a140] def main(): MAP=[root(4,0),root(0,0),root(1,0),root(0,0),root(1,1),root(0,0),root(1,1),root(2,30),root(0,20),root(1,2)] for i in range(len(MAP)): if not ts.is_pressed: MAP[i].action() else: break print("all clear") if __name__ == '__main__': main() メインプログラムで今回のルートをリスト型で指定し、順番に読み込んでいきます。こういった仕組みにするとロボットの動作中に任意で停止させようとタッチセンサーを押しても現在行われている関数の処理を停止するだけでリストの次が読み込まれてしまうため、止まりません。なので読み込み部分にタッチセンサーが押されていないかのチェックと、もし押されているならリストの読み取り自体を終了させるようにしました。 ** プログラムのまとめ [#z7e38825] これまでの説明とプログラム自体の長さからわかるように課題達成のみを目的とする場合にはここまで作る必要ななかったと個人的には思っています。課題達成だけならライントレースの速度もそれぞれ決めればいいしここまで変数や計算を多用する必要もなく、プログラム作成にかかる時間がもっと短くなったと思います。ですがプログラムを作るのであれば汎用性がある方がいいと思っているので今回のようなプログラムにしました。次回の課題では同じようにしようと考えていますが、グループ内で見せたらわかりずらいといわれたのでもう少し考える必要があると思います。
