![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
#author("2020-02-10T17:55:31+09:00","LaiHen","LaiHen") #author("2020-02-10T18:01:45+09:00","LaiHen","LaiHen") ---- 目次 #contents ---- * 課題��「ボール運搬ロボット」 [#m7fda309] 下の図の点C,E,Fに置かれた缶の上のボールを点C',E',F'に置かれた缶の上まで運ぶ~ &ref(2019b/Member/LaiHen/Mission3/2019b-mission2.png,70%,コースの全体図); * ロボットについて [#b2d9ff38] ** ロボットの構造について [#p337419d] 今回の課題ではEV3本体を2台使ってロボットを作成できるため様々なアイデアが浮かびました。~ -1つ目のアイデアは2台を使って1つのロボットを作成する方法 -2つ目のアイデアは2つのロボットを作成する方法です~ この2つのアイデアについて考えていきます。~ 1つ目はライントレースに使うモーター２つ以外は自由に使えるため様々なことができます。しかし、欠点として構造が複雑になってしまうこと、ロボット自体の巨大化、プログラムの複雑化などが挙がってきます。~ 2つ目はライントレースにモーターを2つ使うためロボットそれぞれで自由に使えるのはミディアムモーター1つのみです。そのため様々なことをすることはできません。しかし、構造は単純になりますし、小型化することができる利点もあります。~ ここで今回の課題について考えてみると今回ロボットがするのは缶の上にあるボールを缶の上に運ぶといった動作です。移動についてはライントレースするのでいいとしてボールを缶から缶に運ぶためのつかむ機能が必要です。ここで重要なのがボールの高さが運搬前後で変化していないということです。したがって、ミディアムモーター１つでボールをつかむという動作ができれば、単純かつ小型のロボットの作成ができるということになります。~ こうした考えから下の写真のような同じ型にロボットの作成をすることになりました。~ &ref(2019b/Member/LaiHen/Mission3/lobo1.jpg,40%,ロボット);~ 以下でこのロボットの工夫点を説明していきます。 *** アームの小型化 [#ke64d09d] ロボットの小型化で最も重要なのがアームです。このアームをできるだけコンパクトにする方法がないか試行錯誤したところ、歯車を使った構造を思いつきました。またミディアムモーターを本体の下に収めることで予想以上に小型化できるとともに、ボールを取ろうと缶に近づいた際にロボットが缶にあたらないようにすることができました。~ &ref(2019b/Member/LaiHen/Mission3/lobo2.jpg,30%,ロボット); &ref(2019b/Member/LaiHen/Mission3/lobo3.jpg,30%,ロボット);~ *** センサーの設置 [#p7aab33a] ロボットの小型化で最も苦労したのが超音波センサーの設置場所です。小型化によって空いているスペースが少なく、うまく設置できず、一時期は苦肉の策としてロボットの後ろに設置していた時もありました。しかし、カラーセンサーの近くに設置できないかとやってみると下の写真のようにうまく設置できました。また超音波センサーが斜めになったことで真っ直ぐに設置していた時よりも缶をとらえてくれるようになりました。~ &ref(2019b/Member/LaiHen/Mission3/lobo4.jpg,10%,ロボット);~ ** ロボットのまとめ [#o50710eb] ロボットの作成は相方がメインでやっていたので詳しく説明することはできませんでしたが、ロボットの小型化を目指し、今回のロボットが完成しました。 * プログラムについて [#oab3b1ff] ** プログラムのシステムについて [#w0083243] 今回の課題では同じロボットを２つ用意しているため、1台で缶まで進みボールを回収、その後目的の缶の場所まで進みボールを置くという一連の動作を行う必要があります。また2台で行うためそれぞれがどこのボールを担当するかなどの設定も必要です。そのためには大きく分けて2つのシステムが必要になります。 + 線をたどっていくシステム（ライントレース） + 缶の配置や道などのマップ管理システム + ルート決定システム~ それぞれのシステムについての説明をしていきます。 *** ライントレースシステム [#l9962598] このシステムについては課題�△悩鄒�したライントレースの基準を任意で変更できるようにしたプログラムがあるためそれを流用するとともに、なるべく1つの関数で管理できるように改良することで左右によって関数を変更する必要をなくします。 *** マップ管理システム [#u91678d0] このシステムについては様々な方法が思いつき試行錯誤を重なましたが、うまくプログラムを組むことができず難航しました。最終的には与えられた地図のうち缶がある都合で通れない場所や不必要な場所などを消し、効率よくライントレースができるようにライントレースのシステムで判断できる交差点と直角カーブの点以外を除いた専用の地図を作ることにしました。この作業を図を交えて説明していきます。~ &ref(2019b/Member/LaiHen/Mission3/2019b-mission3_1.png,70%,専用マップ��);~ まず缶の位置と2つのロボットの初期地点である点A,A'から考えてどういった道で進むかを考えます~ &ref(2019b/Member/LaiHen/Mission3/2019b-mission3_2.png,70%,専用マップ��);~ そうして上の図の黄色で塗られた部分は、缶があって通れないところか効率的に運ぶ際につかわない道を表しています。これらを除きライントレースで判定できる交差点と直角カーブ、缶がある点に着目し必要最小限のマップを作成します。~ &ref(2019b/Member/LaiHen/Mission3/2019b-mission3_3.png,70%,専用マップ��);~ そうしてできたのが、上の図のマップになります。四角は点を表していて、赤色で塗られた四角はボールが置かれている場所で、青色で塗られた四角はボールを運んで行っておく場所になります。また、元のアルファベットではやりにくいのでAからPまで振り分けました。~ *** ルート決定システム [#scddaa10] 先ほど作成したマップを参考にルート決定システムを作成していくのですが、最初は座標平面に置き換えて進行方向などをベクトルで扱おうとしたのですが、いい方法が思いつかず、今回作成したのはあまりきれいではありませんが、各点で関数を作成しロボットの持つIDでどの道に行くかの条件分岐をさせていく仕組みにしました。ここで上げたロボットのIDとは２つの状態を表すアルファベットを組み合わせたものです。~ + ボールを回収するのか設置するのか + どこの缶に向かうのか~ この２つでボールを回収しに行っているときは「ｘ」、ボールを設置しに行くときは「ｙ」で表し向かう先の缶のアルファベットを組み合わせてロボットのIDを作成します。例えば点Aのボールを回収しに行くロボットのIDは「xa」となります。このようなIDを作成することで条件分岐の際にいちいち目的地からルートを考えなくても回収したいならこっちといったようにプログラムを簡単にすることができるとともに、設定のミスが減り本番での失敗を防ぐこともできます。~ また、点と点の移動の際にどちらを基準にしてライントレースするかの指定はベクトルと簡単な複素数を用いて判断させることにし、下の図の右上のように上下左右に数字を振りそれをもとにプログラムを書いていきます。~ &ref(2019b/Member/LaiHen/Mission3/2019b-mission3_4.png,70%,方向);~ ** プログラムの説明 [#ta9d6f93] 先の説明でのシステムをもとにプログラムを作成していきます。今回は2台あるためプログラムは2種類必要ですが、同時にボールの運搬をしていくので接触を防ぐための停止や作業の終了プログラムの位置の違いはありますが、もともと1台で一通りこなす必要があるためプログラムはほとんど変わりません。~ - 1台目のプログラム #!/usr/bin/env python3 from ev3dev2.motor import * from ev3dev2.sensor import * from ev3dev2.sensor.lego import * from ev3dev2.sound import * from time import * Wmax,Bmax,spmax,spmin,brockch,tst,balltime=75,5,8,4,6,2.3,1 tw = MoveTank(OUTPUT_A, OUTPUT_D) Mm= MediumMotor(OUTPUT_B) 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] ballst=[1,1,1,0,0,0] snd=Sound() 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 accomand: def __init__(self,line,cltyp,sttime,endtime): self.line,self.cltyp,self.sttime,self.endtime=line,cltyp,sttime,endtime def sendcomand(self): action(self.line,self.sttime,self.endtime) closs(self.cltyp,self.line) class Vector2: def __init__(self,x,y): self.x,self.y=x,y self.comp=complex(x,y) def disp(self): print(self.x,",",self.y) def fukusosuu(self): print(self.comp) class houkou: def __init__(self,hou): self.hou=hou self.x,self.y=0,0 self.kakutei() def kakutei(self): if self.hou==1: self.x=1 elif self.hou==2: self.y=1 elif self.hou==3: self.x=-1 elif self.hou==4: self.y=-1 self.ch=complex(self.x,self.y) class st: def __init__(self): self.stt="xa" self.line,self.point=0,0 def go(self): print("0") def linekizyun(linehou,pointhou): a=houkou(linehou) b=houkou(pointhou) che=Vector2(0,1) if a.ch==(b.ch*che.comp): print("left") ty=0 else: print("ri") ty=1 return ty def vector2math(vec1,vec2): avec=Vector2(vec1.x+vec2.x,vec1.y+vec2.y) return avec def action(line,stti,enti): sttime1,notime1 = time(),time() bcon=0 while not (ts.is_pressed) and not ((notime1-sttime1)>enti): cvp = cs.reflected_light_intensity MLT=Linetorace(cvp) if MLT.colortyp==4: bcon=bcon+1 else: bcon=0 notime1=time() if (stti <= (notime1-sttime1) <=enti) and (bcon== brockch): break if line==0: tw.on(MLT.lesp,MLT.risp) elif line==1: tw.on(MLT.risp,MLT.lesp) else: tw.on(-MLT.lesp,-MLT.risp) sleep(0.1) tw.off() def closs(pp,line): cvp= cs.reflected_light_intensity MLT = Linetorace(cvp) if line==0: tw.on(-MLT.lesp,-MLT.risp) else: tw.on(-MLT.risp,-MLT.lesp) sleep(0.1*brockch) tw.off() if pp==0: action(0,tst,tst+0.1) else: if pp==1: action(0,tst,tst+0.1) elif pp==2: tw.on_for_seconds(spmax,spmax,1) else: action(1,tst,tst+0.1) tw.off() def ballaction(tp): face=0 while not (ts.is_pressed): cvp= cs.reflected_light_intensity MLT= Linetorace(cvp) tw.on(MLT.risp,MLT.lesp) txle= us.distance_centimeters if txle<=4: tw.on_for_seconds(4,4,0.9) face=1 break sleep(0.1) tw.off() if face==1: print("check") if tp==0: Mm.on_for_rotations(20,0.3) tw.on_for_seconds(-15,-15,1.1) tw.on_for_degrees(-4,4,360) else: Mm.on_for_rotations(-10,0.3) tw.on_for_seconds(-15,-15,1.1) tw.on_for_degrees(-4,4,360) def point_a(stas): print("a") root=accomand(linekizyun(1,4),1,0,30) root.sendcomand() stas.stt="yl" if not (ts.is_pressed): point_b(stas) def point_b(stas): print("b") if stas.stt in "xa": ballaction(0) if not (ts.is_pressed): point_a(stas) elif stas.stt in "y" or "xg" or "xh": root=accomand(linekizyun(2,1),0,0,30) root.sendcomand() if not (ts.is_pressed): point_c(stas) def point_c(stas): sleep(15) print("c") if stas.stt in "x": root=accomand(linekizyun(2,3),3,0,30) root.sendcomand() if not (ts.is_pressed): point_b(stas) elif stas.stt in "y" or "yl": root=accomand(linekizyun(2,1),3,0,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) def point_d(stas): print("d") if stas.stt in "xa": root=accomand(linekizyun(2,3),3,0,30) root.sendcomand() if not (ts.is_pressed): point_c(stas) elif stas.stt in "yl": root=accomand(linekizyun(4,2),2,0,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) elif stas.stt in "yn": root=accomand(linekizyun(4,2),3,2,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) elif stas.stt in "yp": root=accomand(linekizyun(4,2),3,2,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) elif stas.stt in "xg": root=accomand(linekizyun(3,2),2,2,30) root.sendcomand() if not (ts.is_pressed): point_e(stas) elif stas.stt in "xh": root=accomand(linekizyun(3,2),3,2,30) root.sendcomand() if not (ts.is_pressed): point_e(stas) def point_e(stas): print("e") if stas.stt in "xg": root=accomand(linekizyun(3,2),3,0,30) root.sendcomand() if not (ts.is_pressed): point_f(stas) if stas.stt in "xh": ballaction(0) if not (ts.is_pressed): point_h(stas) elif stas.stt in "yp" or "yn": root=accomand(linekizyun(3,4),1,0,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) def point_f(stas): print("f") if stas.stt in "xg": ballaction(0) if not (ts.is_pressed): point_g(stas) elif stas.stt in "yp": root=accomand(linekizyun(3,4),2,0,30) root.sendcomand() if not (ts.is_pressed): point_e(stas) def point_g(stas): print("g") stas.stt="yp" root=accomand(linekizyun(3,4),2,0,30) root.sendcomand() if not (ts.is_pressed): point_e(stas) def point_h(stas): print("h") stas.stt="yn" root=accomand(linekizyun(3,4),1,0,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) def point_i(stas): print("i") if stas.stt in "xa": root=accomand(linekizyun(2,3),2,2,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) elif stas.stt in "yl": root=accomand(linekizyun(4,1),3,0,30) root.sendcomand() if not (ts.is_pressed): point_j(stas) elif stas.stt in "yn": root=accomand(linekizyun(1,4),3,2,30) root.sendcomand() if not (ts.is_pressed): point_m(stas) elif stas.stt in "yp": root=accomand(linekizyun(1,4),2,2,30) root.sendcomand() if not (ts.is_pressed): point_m(stas) elif stas.stt in "xg": root=accomand(1,3,2,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) elif stas.stt in "xh": root=accomand(linekizyun(2,3),3,2,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) def point_j(stas): print("j") if stas.stt in "xg": root=accomand(linekizyun(4,3),3,0,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) elif stas.stt in "y" or "yl": root=accomand(linekizyun(4,1),3,0,30) root.sendcomand() if not (ts.is_pressed): point_k(stas) def point_k(stas): print("k") if stas.stt in "yl": ballaction(1) if not (ts.is_pressed): point_l(stas) elif stas.stt in "x" or "xg": root=accomand(linekizyun(4,3),0,0,30) root.sendcomand() if not (ts.is_pressed): point_j(stas) def point_l(stas): print("l") stas.stt="ee" root=accomand(linekizyun(3,2),1,0,30) root.sendcomand() if not (ts.is_pressed): endd() def point_m(stas): print("m") if stas.stt in "yp": root=accomand(linekizyun(1,4),3,0,30) root.sendcomand() if not (ts.is_pressed): point_o(stas) elif stas.stt in "yn": ballaction(1) if not (ts.is_pressed): endd() stas.stt="ee" elif stas.stt in "xa" or "xh": root=accomand(linekizyun(1,2),1,2,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) def point_n(stas): print("n") stas.stt="xa" root=accomand(linekizyun(1,2),1,0,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) def point_o(stas): print("o") if stas.stt in "yp": ballaction(1) if not (ts.is_pressed): point_p(stas) elif stas.stt in "xa": root=accomand(linekizyun(1,2),2,0,30) root.sendcomand() if not (ts.is_pressed): point_m(stas) def point_p(stas): print("p") stas.stt="xh" root=accomand(linekizyun(1,2),2,0,30) root.sendcomand() if not (ts.is_pressed): point_m(stas) def endd(): snd.speak("finish") def main(): snd.speak("start") lobo=st() point_b(lobo) snd.beep() if __name__ == '__main__': main() - 2台目のプログラム #!/usr/bin/env python3 from ev3dev2.motor import * from ev3dev2.sensor import * from ev3dev2.sensor.lego import * from ev3dev2.sound import * from time import * Wmax,Bmax,spmax,spmin,brockch,tst,balltime=75,5,8,4,6,2.3,1 tw = MoveTank(OUTPUT_A, OUTPUT_D) Mm= MediumMotor(OUTPUT_B) 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] ballst=[1,1,1,0,0,0] snd=Sound() 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 accomand: def __init__(self,line,cltyp,sttime,endtime): self.line,self.cltyp,self.sttime,self.endtime=line,cltyp,sttime,endtime def sendcomand(self): action(self.line,self.sttime,self.endtime) closs(self.cltyp,self.line) class Vector2: def __init__(self,x,y): self.x,self.y=x,y self.comp=complex(x,y) def disp(self): print(self.x,",",self.y) def fukusosuu(self): print(self.comp) class houkou: def __init__(self,hou): self.hou=hou self.x,self.y=0,0 self.kakutei() def kakutei(self): if self.hou==1: self.x=1 elif self.hou==2: self.y=1 elif self.hou==3: self.x=-1 elif self.hou==4: self.y=-1 self.ch=complex(self.x,self.y) class st: def __init__(self): self.stt="xg" self.line,self.point=0,0 def go(self): print("0") def linekizyun(linehou,pointhou): a=houkou(linehou) b=houkou(pointhou) che=Vector2(0,1) if a.ch==(b.ch*che.comp): print("left") ty=0 else: print("ri") ty=1 return ty def vector2math(vec1,vec2): avec=Vector2(vec1.x+vec2.x,vec1.y+vec2.y) return avec def action(line,stti,enti): sttime1,notime1 = time(),time() bcon=0 while not (ts.is_pressed) and not ((notime1-sttime1)>enti): cvp = cs.reflected_light_intensity MLT=Linetorace(cvp) if MLT.colortyp==4: bcon=bcon+1 else: bcon=0 notime1=time() if (stti <= (notime1-sttime1) <=enti) and (bcon== brockch): break if line==0: tw.on(MLT.lesp,MLT.risp) elif line==1: tw.on(MLT.risp,MLT.lesp) else: tw.on(-MLT.lesp,-MLT.risp) sleep(0.1) tw.off() def closs(pp,line): cvp= cs.reflected_light_intensity MLT = Linetorace(cvp) if line==0: tw.on(-MLT.lesp,-MLT.risp) else: tw.on(-MLT.risp,-MLT.lesp) sleep(0.1*brockch) tw.off() if pp==0: action(0,tst,tst+0.1) else: if pp==1: action(0,tst,tst+0.1) elif pp==2: tw.on_for_seconds(spmax,spmax,1) else: action(1,tst,tst+0.1) tw.off() def ballaction(tp): face=0 while not (ts.is_pressed): cvp= cs.reflected_light_intensity MLT= Linetorace(cvp) tw.on(MLT.risp,MLT.lesp) txle= us.distance_centimeters if txle<=4: tw.on_for_seconds(4,4,0.9) face=1 break sleep(0.1) tw.off() if face==1: print("check") if tp==0: Mm.on_for_rotations(20,0.3) tw.on_for_seconds(-15,-15,1.1) tw.on_for_degrees(-4,4,360) else: Mm.on_for_rotations(-10,0.3) tw.on_for_seconds(-15,-15,1.1) tw.on_for_degrees(-4,4,360) def point_a(stas): print("a") root=accomand(linekizyun(1,4),1,0,30) root.sendcomand() stas.stt="yl" if not (ts.is_pressed): point_b(stas) def point_b(stas): print("b") if stas.stt in "xa": ballaction(0) if not (ts.is_pressed): point_a(stas) elif stas.stt in "y" or "xg" or "xh": root=accomand(linekizyun(2,1),0,0,30) root.sendcomand() if not (ts.is_pressed): point_c(stas) def point_c(stas): print("c") if stas.stt in "x": root=accomand(linekizyun(2,3),3,0,30) root.sendcomand() if not (ts.is_pressed): point_b(stas) elif stas.stt in "y" or "yl": root=accomand(linekizyun(2,1),3,0,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) def point_d(stas): print("d") if stas.stt in "xa": root=accomand(linekizyun(2,3),3,0,30) root.sendcomand() if not (ts.is_pressed): point_c(stas) elif stas.stt in "yl": root=accomand(linekizyun(4,2),2,0,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) elif stas.stt in "yn": root=accomand(linekizyun(4,2),3,2,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) elif stas.stt in "yp": root=accomand(linekizyun(4,2),3,2,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) elif stas.stt in "xg": root=accomand(linekizyun(3,2),2,2,30) root.sendcomand() if not (ts.is_pressed): point_e(stas) elif stas.stt in "xh": root=accomand(linekizyun(3,2),3,2,30) root.sendcomand() if not (ts.is_pressed): point_e(stas) def point_e(stas): print("e") if stas.stt in "xg": root=accomand(linekizyun(3,2),3,0,30) root.sendcomand() if not (ts.is_pressed): point_f(stas) if stas.stt in "xh": ballaction(0) if not (ts.is_pressed): point_h(stas) elif stas.stt in "yp" or "yn": root=accomand(linekizyun(3,4),1,0,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) def point_f(stas): print("f") if stas.stt in "xg": ballaction(0) if not (ts.is_pressed): point_g(stas) elif stas.stt in "yp": root=accomand(linekizyun(3,4),2,0,30) root.sendcomand() if not (ts.is_pressed): point_e(stas) def point_g(stas): sleep(20) print("g") stas.stt="yp" root=accomand(linekizyun(3,4),2,0,30) root.sendcomand() if not (ts.is_pressed): point_e(stas) def point_h(stas): print("h") stas.stt="yn" root=accomand(linekizyun(3,4),1,0,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) def point_i(stas): print("i") if stas.stt in "xa": root=accomand(linekizyun(2,3),2,2,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) elif stas.stt in "yl": root=accomand(linekizyun(4,1),3,0,30) root.sendcomand() if not (ts.is_pressed): point_j(stas) elif stas.stt in "yn": root=accomand(linekizyun(1,4),3,2,30) root.sendcomand() if not (ts.is_pressed): point_m(stas) elif stas.stt in "yp": root=accomand(linekizyun(1,4),2,2,30) root.sendcomand() if not (ts.is_pressed): point_m(stas) elif stas.stt in "xg": root=accomand(1,3,2,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) elif stas.stt in "xh": root=accomand(linekizyun(2,3),3,2,30) root.sendcomand() if not (ts.is_pressed): point_d(stas) def point_j(stas): print("j") if stas.stt in "xg": root=accomand(linekizyun(4,3),3,0,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) elif stas.stt in "y" or "yl": root=accomand(linekizyun(4,1),3,0,30) root.sendcomand() if not (ts.is_pressed): point_k(stas) def point_k(stas): print("k") if stas.stt in "yl": ballaction(1) if not (ts.is_pressed): point_l(stas) elif stas.stt in "x" or "xg": root=accomand(linekizyun(4,3),0,0,30) root.sendcomand() if not (ts.is_pressed): point_j(stas) def point_l(stas): print("l") stas.stt="xg" root=accomand(linekizyun(3,2),1,0,30) root.sendcomand() if not (ts.is_pressed): point_k(stas) def point_m(stas): print("m") if stas.stt in "yp": root=accomand(linekizyun(1,4),3,0,30) root.sendcomand() if not (ts.is_pressed): point_o(stas) elif stas.stt in "yn": ballaction(1) if not (ts.is_pressed): endd() stas.stt="ee" elif stas.stt in "xa" or "xh": root=accomand(linekizyun(1,2),1,2,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) def point_n(stas): print("n") stas.stt="xa" root=accomand(linekizyun(1,2),1,0,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) def point_o(stas): print("o") if stas.stt in "yp": ballaction(1) if not (ts.is_pressed): point_p(stas) elif stas.stt in "xa": root=accomand(linekizyun(1,2),2,0,30) root.sendcomand() if not (ts.is_pressed): point_m(stas) def point_p(stas): print("p") stas.stt="xh" root=accomand(linekizyun(1,2),2,0,30) root.sendcomand() if not (ts.is_pressed): point_m(stas) def endd(): snd.speak("finish") def main(): snd.speak("start") lobo=st() point_j(lobo) snd.beep() if __name__ == '__main__': main() 以上のプログラムをいくつかに分けて説明していきます。 *** �．廛蹈哀薀爐了�前設定と基準値の算出 [#jb56f6b0] この部分はほとんど課題�△汎韻犬任后� #!/usr/bin/env python3 from ev3dev2.motor import * from ev3dev2.sensor import * from ev3dev2.sensor.lego import * from ev3dev2.sound 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] - これで今後使う変数の準備ができました。プログラムを起動してすぐに変数を決めるようにしたので関数内などで毎回計算する必要がないので各関数の処理がわずかですが速くすることができます。 *** �▲薀ぅ鵐肇譟璽考僂離�ラス [#vfd492c2] このクラスは課題�△悩鄒�したものをそのまま流用しました。 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段階のうちどれに該当するかを判定する。その後それぞれの段階ごとに左右のモーターの速度を決める。~ このクラスはセンサーの値から左右の速度、段階などを取得できるのでカラーセンサー関係を一括で管理することができます。~ このクラスのメインである段階分けと左右の速度決定を説明していきます。 + 段階分け カラーセンサーの値を以下の表と照らし合わせて段階数を決めます。ここでの範囲は灰色を基準に対称になるようになっています。 |色の段階|白色|白色と灰色の中間|灰色|黒色と灰色の中間|黒色| |段階数|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つで速度を求めることができるのです。~ -- 左側の時は右の速度を右のモーターに左の速度を左のモーターに送る -- 右側の時は右の速度を左のモーターに左の速度を右のモーターに送る *** ��ライントレース関係の一括管理用クラス [#sbb279a1] class accomand: def __init__(self,line,cltyp,sttime,endtime): self.line,self.cltyp,self.sttime,self.endtime=line,cltyp,sttime,endtime def sendcomand(self): action(self.line,self.sttime,self.endtime) closs(self.cltyp,self.line) このクラスを用いてライントレースと交差点または直角カーブでぶつかった後の動きをまとめて指定できるようにしました。今回点と点のライントレースをすると間の移動と点に到達後の動きはまとめて指定できる方が望ましいと思いこのようなクラスを作成しました。~ このクラスではライントレースの基準、交差点での動き、交差点判定開始までの時間、ライントレースを続ける時間の4つを指定します。 *** �ぅ戰�トルに使うクラス [#d943504f] class Vector2: def __init__(self,x,y): self.x,self.y=x,y self.comp=complex(x,y) def disp(self): print(self.x,",",self.y) def fukusosuu(self): print(self.comp) class houkou: def __init__(self,hou): self.hou=hou self.x,self.y=0,0 self.kakutei() def kakutei(self): if self.hou==1: self.x=1 elif self.hou==2: self.y=1 elif self.hou==3: self.x=-1 elif self.hou==4: self.y=-1 self.ch=complex(self.x,self.y) この2つのクラスでベクトルの設定と、上下左右の方向を表すベクトルの設定を行います。 *** �ゥ蹈椒奪箸�ID設定用クラス [#n203d7bb] class st: def __init__(self): self.stt="xg" self.line,self.point=0,0 def go(self): print("0") このクラスを作成して、各点での動きの管理をできるようにしてます。 ここではIDの初期設定は「xg」となっていますが、もう一方の初期設定は「xa」となっておりスタートして同じ目標に行かないようにしています。 *** �κＡ膿瑤鰺僂い織薀ぅ鵐肇譟璽垢虜険Δ隆霆爐侶萃� [#be4e8913] def linekizyun(linehou,pointhou): a=houkou(linehou) b=houkou(pointhou) che=Vector2(0,1) if a.ch==(b.ch*che.comp): print("left") ty=0 else: print("ri") ty=1 return ty ある道は決まった側をライントレースして進んでほしいですが、ロボットの進む向きによってそれが右なのか左なのか変わってしまいます。そこでマップでその線の上下または左右のどっちの側をライントレースしてほしいかを指定し、その方向が進む向きを基準にすると左右どちらなのかを判定するのがこの関数です。方向をベクトルで表しているため、左右の判定は複素数を用いて計算しています。 *** �Д薀ぅ鵐肇譟璽考儡愎� [#p3613cc8] def action(line,stti,enti): sttime1,notime1 = time(),time() bcon=0 while not (ts.is_pressed) and not ((notime1-sttime1)>enti): cvp = cs.reflected_light_intensity MLT=Linetorace(cvp) if MLT.colortyp==4: bcon=bcon+1 else: bcon=0 notime1=time() if (stti <= (notime1-sttime1) <=enti) and (bcon== brockch): break if line==0: tw.on(MLT.lesp,MLT.risp) elif line==1: tw.on(MLT.risp,MLT.lesp) else: tw.on(-MLT.lesp,-MLT.risp) sleep(0.1) tw.off() プログラムの構造は課題�△箸曚箸鵑鋲韻犬任垢�、今回はそこに左右を引数で指定できるようにし、1つの関数でライントレースを行えるようにしました。 3つの引数（ライントレースの基準、交差点判定開始までの時間、関数の処理の制限時間）を使う関数です。この関数内で行う処理は大きく分けると - 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秒おきに色のチェックをするため曲線部分での交差点判定のミスがありました。なので、カウントするのは黒色の時だけにし、もしそれ以外の段階が来たらカウントをリセットする仕組みすることで判定のミスを抑えました。 *** ��直角カーブと交差点での行動用関数 [#q91b87c6] def closs(pp,line): cvp= cs.reflected_light_intensity MLT = Linetorace(cvp) if line==0: tw.on(-MLT.lesp,-MLT.risp) else: tw.on(-MLT.risp,-MLT.lesp) sleep(0.1*brockch) tw.off() if pp==0: action(0,tst,tst+0.1) else: if pp==1: action(0,tst,tst+0.1) elif pp==2: tw.on_for_seconds(spmax,spmax,1) else: action(1,tst,tst+0.1) tw.off() 引数でカーブ、交差点を左折、右折、直進の4つを指示しそれぞれの処理を実行する関数です。ここでの処理は + 現在のカラーセンサーの値からさっきまでの道を戻る + その後4つそれぞれの動きを実行する といった流れです。~ なぜ交差点での処理で一旦道を戻るのはこの関数が呼び出された時のロボットの状況にあります。この関数を呼ぶということは交差点にいるということです。この時ロボットはライントレースで黒色の判定なので左に曲がっています。次の処理がカーブや左折であれば問題ありませんが直進や右折の場合は動きが複雑にあってしまいます。これを簡単にするために交差点判定のカウントを始める直前のロボットがおおむね黒線に対して平行の状態まで道を戻る処理を行います。~ 後は引数の値で処理を変えて4つの行動を分けます。 ++ カーブ~ カーブの場合は一旦止まる必要はないのでそのまま黒線の左側を基準にライントレースを行います。 ++ 交差点を左折~ 交差点なのでカーブと同様の処理をします。 ++ 交差点を直進~ 交差点なので左右のモーターを最大速度で1秒間動かします。 ++ 交差点を右折~ 交差点なので黒線の右側を基準にライントレースを行います。 *** ��ボール関係の行動用関数 [#xc2f5d47] def ballaction(tp): face=0 while not (ts.is_pressed): cvp= cs.reflected_light_intensity MLT= Linetorace(cvp) tw.on(MLT.risp,MLT.lesp) txle= us.distance_centimeters if txle<=4: tw.on_for_seconds(4,4,0.9) face=1 break sleep(0.1) tw.off() if face==1: print("check") if tp==0: Mm.on_for_rotations(20,0.3) tw.on_for_seconds(-15,-15,1.1) tw.on_for_degrees(-4,4,360) else: Mm.on_for_rotations(-10,0.3) tw.on_for_seconds(-15,-15,1.1) tw.on_for_degrees(-4,4,360) この関数でボールの回収と設置を行います。この関数は缶がある線に来た時に飛び出され、超音波センサーで距離を測りながら一定の長さに来るまではライントレースをして缶に近づいていきます。一定の距離に来たらライントレースをやめます。しかし、超音波センサーの都合で完全にロボットが缶に接近できていないのでその調整をし、その後回収なのか設置なのかを引数から判断し、アームを動かします。アームの動作が終了したら次の作業のためにUターンしないといけないのですが、今回の課題では缶にロボットがぶつかって缶を動かしてしまうと減点になってしまうので一旦バックをしあたらない距離まで離れてからUターンをするようにしました。 *** ��マップ関数 [#q841576d] 専用のマップをプログラムで管理していくための関数を作成し、ロボットの動きを決定するようにします。まずマップにある点A〜点Pまでを2つに分けます。 + 缶が設置してある点（A,G,H,L,N,P） + 中間点または分岐点~ - 缶が設置してある点に来たということはボールの回収または設置をし終え次の缶のところに向かいます。なのでこの点のプログラム（例として点Aのプログラム）で説明していきます。 def point_a(stas): print("a") root=accomand(linekizyun(1,4),1,0,30) root.sendcomand() stas.stt="yl" if not (ts.is_pressed): point_b(stas) -- 点Aの次の点であるBに向かうためのライントレースを行います root=accomand(linekizyun(1,4),1,0,30) root.sendcomand() -- 点Aで回収したボールを点Lに置きに行くためロボットのIDを「yl」に設定します。 stas.stt="yl" -- 点Aでの処理が終わったので次の点である点Bの関数を呼び出します if not (ts.is_pressed): point_b(stas) - 中間点または分岐点ではロボットのIDに応じて次の点と行動を決定します。この点でのプログラム（例として点Bのプログラム）で説明していきます。 def point_b(stas): print("b") if stas.stt in "xa": ballaction(0) if not (ts.is_pressed): point_a(stas) elif stas.stt in "y" or "xg" or "xh": root=accomand(linekizyun(2,1),0,0,30) root.sendcomand() if not (ts.is_pressed): point_c(stas) -- 点BではIDが「xa」の時は点Aに進み、ID「y_」,「xg」,「xh」の時は点Cに進みます。この時点点Aに進むときはライントレースをするのではなく、ボールの回収用の関数を呼び出します。 - この2種類の点を使いすべての点の関数を作成します。すべての関数を定義することでマップが作成でき、行動を順番に呼び出されていくことができます。 *** ��運搬終了用関数 [#t7422e7d] def point_m(stas): print("m") if stas.stt in "yp": root=accomand(linekizyun(1,4),3,0,30) root.sendcomand() if not (ts.is_pressed): point_o(stas) elif stas.stt in "yn": ballaction(1) if not (ts.is_pressed): endd() stas.stt="ee" elif stas.stt in "xa" or "xh": root=accomand(linekizyun(1,2),1,2,30) root.sendcomand() if not (ts.is_pressed): point_i(stas) def endd(): snd.speak("finish") ��で説明したマップの関数では1台で3つのボールの運搬をできるようにしています。ですが、本番は2台で3つのボールの運搬をするため、マップ関数の循環から抜け出してプログラムを終了する必要があります。プログラムを終了方法は、現在の点での処理を終了し次の点の関数を呼び出す部分を削除し、その際それがわかるようにEV3から音声を出すようにします。また本来ならこれだけでいいのですが、なぜかロボットのIDを変更しないとうまく終了してくれないのでIDを関係ないものに書きかえます。こうすることで運搬を終了できます。 *** ��メインプログラム [#u379c5d1] def main(): snd.speak("start") lobo=st() point_j(lobo) snd.beep() if __name__ == '__main__': main() プログラムが起動したのをわかりやすくするためにいったん音声が出るようにし、その後ロボットのIDの設定と、マップ関数の呼び出しをします。ここでは「point_j」となっていますが、もう一方のロボットではこの部分は「point_b」になっています。これはスタート位置が違っているための対応になります。最後にマップ関数が終了し完全にプログラムの処理が終了したという信号としてビープ音を鳴らします。 ** プログラムのまとめ [#da77f4c5] 本来この課題では2台の間で通信しないといけないのですが、通信機能の追加を想定せずにプログラムを組み立てたため少し一時停止をさせたりといった微調整だけで2台での作業ができてしまったことや、マップのシステムの作成に難航してしまいできたころには通信機能をつけるような時間の余裕がなくなってしまったなどの理由で2台の通信を行いませんでした。本当ならば通信機能を使ってもっと凝ったことをしようとしていたのですが、残念ながらできませんでした。 * 結果 [#jff54c82] 本番ではロボット本体とプログラムの両方がうまくできていたのか、課題である3つのボールの運搬をすべて行うことができました。ですが、超音波センサーがうまく反応しなかったのか1つの缶だけ想定通りいかず缶が円の外側にはみ出してしまいました。もしかしたらパーフェクトが取れたかもしれなかったので少し残念ですが、ロボコンで1位になれたので頑張ったかいがあったと思います。
