このページの執筆者。その辺に転がってる普通の大学生。謎のプログラムを書く。
プログラムの缶を積む動作担当
chikuwaの前回の課題ページはこちら
2013b/Member/chikuwa/Mission1
主にプログラム担当。Macを自由自在に操り、華麗にプログラムを打ち込んでいく。
簡潔で無駄のないプログラムを書く技術に定評がある。
プログラムの缶を見つける動作担当
pianoman氏の今回の課題のページはこちら
2013b/Member/pianoman/Mission2
主にハード担当。ロボットの半分は彼の優しさでできています。
尚、ロボットの作成や練習には彼の部屋を度々使用させてもらった。本当にありがたいことである。
プログラムのライントレース担当
Redcicudu氏の今回の課題のページはこちら
2013b/Member/Redcicudu/Mission2
前回の失敗を踏まえ、今回はクリアしなければいけない課題を細かく分けてひとつずつクリアさせ
最終的にそれらを統合して課題の解決を目指す。
具体的には今回の課題の難点は次のように分類できる。
・ライントレース
・缶の発見
・缶の積み上げ
・缶を積んだ状態での移動
・2つのNXTの連携
まずはこれらを一つずつつぶしていく。
→クレーン式?フォークリフト式?アームの上げ下げ式?2台に分けて作業分担?
結論:アームの上げ下げ式。因みにアームは水平移動しない。
理由:いろいろと試した結果、ほかの方法ではパーツが足りない。また缶を掴む際にしっかり掴めない。
モーター数:4つ
(タイヤ:2つ、缶の上げ下げ:1つ、缶を掴む,離す:1つ)
NXT本体:2つ(←モーターを4つ使うため)
用いるセンサ:光センサ、超音波センサ
(光センサ⇀ライントレースに使用、超音波センサ⇀缶の発見に使用)
1/12頃に作成
↑NXTが不安定なうえに、前が重くなりがち...
この頃はドライブベースはchikuwa、アームの上げ下げ部はRedcicudu氏、
(写っていないが)アームのキャッチ部はPianoman氏が担当している。
↑こんな風に後輪の代わりにボールを使う方法も最初は考えていた。
しかしボールを転がすというより引きずる感じになってしまったので、この案は却下となった。
上図ではまだアームの掴む部分をつけていないが、もしこの状態でそれをつけると重みで本体が前に傾く可能性がある。
また中央部がスカスカなのでもう一台NXTをつけると重みで機体が沈む。
そこで土台の隙間をさらにうめて、NXTを倒した状態で2つくっつけてみた。
重心を前回より後方にすることで安定度が向上するようにしてみた。
そして次のような完成品が出来上がった。
Φ全体
↑アームをつけた状態で前に倒しても前のめりになることは無かった。
超音波センサは縦に取り付けた方が感度が正確になるらしので、縦に取り付けてみた。
ちなみに、主な製作者はRedcicudu氏である。
NXT本体2つは通信させながら使用する。
役割分担としては
Masater側がタイヤ、光センサ、超音波センサを司り、
Slave側が缶のキャッチ、上げ下げを司る。
Φ腹部
↑底面部は隙間なくパーツで埋まっているので、自重で
タイヤの軸がゆがんだりすることもない。
Φ後部
↑赤丸で示したように小さなタイヤ二つをつけた。これはきちんと360°回転する。
四輪駆動も試してみたが、カーブが曲がり切れなかったため最終的にこの方法を採用した。
試しに動かしてみたところ、ゆっくりではあるがきちんと機体は一回転した。
Φアーム部
↑初めは、キャッチしても缶の重みでアームをすり抜けてしまうことが多かったが、
輪ゴムをつけると安定して掴めるようになった。
使用するロボットが1台と分かった時点で、缶をとる順序、
どこでライントレースするかなどを決定した。
積む缶の個数は4個。(それ以上はさすがにバランスなどの問題で厳しいものがあった。)
またAsahiスーパードライが一番積みやすいらしいが、ロボットの作りが非常に安定している
おかげで缶の種類にこだわる必要性は特に感じられなかった。
下のように前半、後半からそれぞれ缶を積みながら進んでいき、最終的にゴール地点に
缶4つのタワーを2つ作り上げる。
↑前半は紫色、後半は赤色で示した
また缶を積む順番に、それぞれA,B,C,D,E,F,G,Hという番号を
割り振った。
後半で、3個目の缶を掴んだ後、4個目の缶に到達するまでの道が長いので、
缶がずりおちてしまい、4個目がつめないことがあったので、4個目の
缶の直前のT字路を渡った後、いったん缶を持ち直すプログラムを挿入した。
ちなみに4個目を積んだ後の道も長いが、その次は積むのではなく、
ゴールに置くだけなので、特に持ち直す動作は入れていない。
ある程度動きが固まってきたらまずは前半のルート、後半のルート用のプログラムをそれぞれ作成し、
両方と検証に検証を重ねた上で、その2つを一つのプログラムに合成した。
上のメンバー紹介でも少し触れたが、ハードができた後は
それぞれ担当のプログラムを割り振って、作業の効率化を図った。
・主にchikuwa担当
最初は二台のNXTの通信がうまく行くかどうか確かめる目的も兼ねて、
アームを動かす練習をしてみる。
とりあえずは
アームを下ろす→前進→缶をキャッチ→アームを上げる
という非常に単純な動作をさせてみる。
アームを下ろしたあと機体を前進させたのは、機体と缶の間にできる隙間をなくすためである。
また予め、モーターの角度を測るプログラムを用いてアームの上げ下げ、缶のキャッチ・リリースに
必要な角度を計算しておき、命令"RotateMotor"を用いてみた。
✡マスター(M)側
#define CONN 1 #define SIGNALON 11 task main() { RotateMotor(OUT_B,75,4270);//アームを下げる SendRemoteNumber(CONN,MAILBOX1,SIGNALON); /*Sのメールボックス1にSIGNALON(1)を送信*/ Off(OUT_AB);Wait(3000);//3秒間待機 RotateMotor(OUT_A,85,-110);//缶をキャッチ RotateMotor(OUT_B,75,-4270);//アームを上げる }
✡スレイブ(S)側
#define SIGNALON 11 task main() { int msg; while(true){ ReceiveRemoteNumber(MAILBOX1,true,msg); /*Mからメールボックス1にメッセージを受信*/ if(msg==SIGNALON){//もし受け取ったメッセージがSIGNALON(11)なら OnFwd(OUT_AB,60);Wait(1000);//1秒前進 }else{//そうでないなら Off(OUT_AB);//停止 } } }
うん。きちんと通信して、プログラム通りに動いてくれた。
通信がうまく行ったようだ。
Δ下ろし中
モーター回転部(黄色の丸)は、おおよそ赤色で示したような軌跡を描く。
Δ下ろし終わり
赤丸で示したようにこのままだと隙間ができてしまう。
Δ前進
隙間を埋めるため少しだけ前進。
Δキャッチ
離さない様かなり強めの出力でキャッチ。
Δ上げ終わり
ずり落ちることもなく、缶は上部へ
・"RotateMotor"がとても便利、モーターの出力を変えるたびに、
わざわざ動かす時間を変更しなくても良い。
・モーターに直接ギアをつけるのではなく、いくつか歯車を経由してるので、
動きが意外と遅かった。
・二台の通信に少し時間がかかる。
ここまでは1/18(土)までに終わらせた。
次は超音波センサの有効範囲確認のため、そしてスレイブ側からマスター側に値を送る練習のため、
前進→缶を発見→アームを下ろす→前進→缶を掴む→缶を上げる→前進
という前回より少しだけ手間のかかる動作を出来るようにする。
計4回も通信するので少し不安だが、テキストを参考に見様見真似で作ってみた。
計4回の内訳は
M側からS側へ・・・2回
S側からM側へ・・・2回
各NXTが出す指令をまとめると次のようになる。
✡マスター側
#define CONN 1 #define SIGNAL_A 11 #define SIGNAL_B 12 #define SIGNAL_C 13 #define SIGNAL_D 14 #define go_forward OnFwd(OUT_AB,60); task main() { SetSensorLowspeed(S1);//超音波センサーを用意 int msg; int msg_2; /*↑スレイブから送られた値を格納する関数↑*/ go_forward; if(SensorUS(S4)<10){//もし10センチ以内に缶を発見したら SendRemoteNumber(CONN,MAILBOX1,SIGNAL_A); /*Sのメールボックス1にSIGNAL_Aを送信*/ ReceiveRemoteNumber(MAILBOX2,true,msg); /*Sからメールボックス2にメッセージを受信*/ if(msg==SIGNAL_B){ //もしSIGNAL_Bを受信したら OnFwd(OUT_AB,60);Wait(1000);//1秒間前進 SendRemoteNumber(CONN,MAILBOX3,SIGNAL_C); /*Sのメールボックス3にメッセージを送信*/ ReceiveRemoteNumber(MAILBOX4,true,msg_2); /*Sからメールボックス4にメッセージを受信*/ if(msg_2==SIGNAL_D){//もしメッセージがSIGNAL_Dなら go_forward;Wait(2000);//2秒前進 } } } }
✡スレイブ側
#define CONN 1 #define SIGNAL_A 11 #define SIGNAL_B 12 #define SIGNAL_C 13 #define SIGNAL_D 14 task main() { int msg; int msg_2; ReceiveRemoteNumber(MAILBOX1,true,msg); /*Mからメールボックス1にメッセージを受信*/ if(msg==SIGNAL_A){//もしメッセージがSIGNAL_Aなら RotateMotor(OUT_B,75,4270);//アームを下ろす SendResponseNumber(MAILBOX2,SIGNAL_B); /*Mのメールボックス2にSIGNAL_Bを送信*/ Off(OUT_AB);Wait(1000);//1秒間待機 ReceiveRemoteNumber(MAILBOX3,true,msg_2); /*Mからメールボックス3にメッセージを受信*/ if(msg_2==SIGNAL_C){//もしメッセージがSIGNAL_Cなら RotateMotor(OUT_A,85,-110);//缶を掴む RotateMotor(OUT_B,75,-4270);//アームを上げる SendResponseNumber(MAILBOX4,SIGNAL_D); /*Mのメールボックス4にSIGNAL_Dを送信*/ } } }
進む→缶を見つける
までは良かったのだがその後、進みながらアームを下ろし、缶をつかめなかった。
更に自分のアームを缶だと勘違いし、さらにアームを下ろそうとしたのでこれはまずいと思い、
そこでストップさせた。
それぞれの、相手のNXTに値を送る命令"SendRemoteNumber","SendResponseNumber"
の時間を設定していなかったので、送るタイミング,受け取るタイミングがバラバラになってしまい
最初のM→Sの通信一回しかできなかった。結局それぞれ独立してプログラムを実行してしまった。
その後少しプログラムをいじったのだが、何度やってもS側からM側に命令が送れなかった。
このままだと埒があかないので再びピアサポートセンターへ...(授業の関係で、Redcicudu氏
に頼む形となってしまった。非常に面目ない。)
"while(true)"を用いて、常に相手のNXTから命令を受け取るようにしてみた。
また、"SendRemote/ResponseMessage"の数を減らした。
そこで出来上がったのが、次のプログラム。
✡マスター側
#define CONN 1 #define SIGNAL_A 11 #define SIGNAL_B 12 #define SIGNAL_C 13 #define SIGNAL_D 14 #define go_forward OnFwd(OUT_AB,60);//出力60%で前進 task main() { SetSensorLowspeed(S4); int msg; int msg_2; int hakken=0;//←缶を発見した回数をカウントする関数 while(hakken==0){//hakkenが0の時 go_forward; if(SensorUS(S4)<=20){//もし20センチ以内に缶を発見したら hakken++;//hakkenをカウントする SendRemoteNumber(CONN,MAILBOX1,SIGNAL_A); /*SのメールボックスにSIGNAL_Aを送信(これにより、S側でアームを下ろす)*/ PlaySound(SOUND_UP);//目印として音を鳴らす } } /*ここでhakkenの値が1になっているので、次の"while(hakken==1)"に移項*/ while(hakken==1){//hakkenの値が1の時 ReceiveRemoteNumber(MAILBOX2,true,msg); /*Sからメールボックス2にメッセージを受信する*/ if(msg==SIGNAL_B){//もしSIGNAL_Bを受信したら OnFwd(OUT_AB,60);Wait(1000);//1秒間前進(←隙間を埋めるため) SendRemoteNumber(CONN,MAILBOX3,SIGNAL_C); /*Sのメールボックス3にメッセージを送信(これによりS側で缶をつかみ、アームを上げる)*/ Off(OUT_AB);//機体停止 } if(msg==SIGNAL_D){//もしSIGNAL_Dを受信したら go_forward;Wait(2000);//2秒前進 Off(OUT_AB); Wait(5000);//5秒待機 } Wait(100);//0.1秒に一回受信する }//while(hakken==1) }//task main
✡スレイブ側
#define CONN 1 #define SIGNAL_A 11 #define SIGNAL_B 12 #define SIGNAL_C 13 #define SIGNAL_D 14 task main() { while(true){ int msg; ReceiveRemoteNumber(MAILBOX1,true,msg); /*Mからメールボックス1にメッセージを受信*/ if(msg==SIGNAL_A){//もしメッセージがSIGNAL_Aなら RotateMotor(OUT_B,75,4100);//アームを下ろす SendResponseNumber(MAILBOX2,SIGNAL_B); /*MのメールボックスにSIGNAL_Bを送信*/ } if(msg==SIGNAL_C){//もしメッセージがSIGNAL_Cなら RotateMotor(OUT_A,85,-110);//缶を掴む RotateMotor(OUT_B,75,-4100);//アームを上げる SendResponseNumber(MAILBOX4,SIGNAL_D); /*MのメールボックスにSIGNAL_Dを送信*/ } }//while(true) }//task main
う〜ん、うまく行かない...
何度やっても、S側からM側へ値が送れない...
困った。
そもそもメールボックスとはM側、S側にそれぞれ独立して存在しているのではなく
共有しあうものらしい。つまり、M側から命令を送る場合とS側から命令を送る場合でメールボックスを
使い分ける必要があるようだ。
そこでpianoman氏がその点を考慮し、S側からも命令を送れる次のようなプログラムを作成して下さった。
このプログラムのおかげで我々の使える技術は飛躍的に増加した。
↓"koushin.nxc"
/* sm.h */ /* 送信と確認 */ sub sendmsg(int conn,int mb1,int mb2,string msg,string rps) { SendRemoteString(conn,mb1,msg); string rmsg = ""; while(rmsg == "") { ReceiveRemoteString(mb2,true,rmsg); TextOut(0,LCD_LINE1,"Wait until slave /n completes tasks..."); Wait(1000); } } sub btcheck(int conn) { while(!BluetoothStatus(conn) == NO_ERR) { ResetScreen(); TextOut(0,LCD_LINE1,"Something wrong"); Wait(1000); } } sub receive(int mb1,string rd) { string msg; while(msg != rd) { ReceiveRemoteString(mb1,true,msg); ResetScreen(); TextOut(0,LCD_LINE1,"Receiving..."); Wait(1000); } ResetScreen(); TextOut(0,LCD_LINE1,"I got it!!"); Wait(1000); } sub respond(int mb2,string reply) { SendResponseString(mb2,reply); ResetScreen(); TextOut(0,LCD_LINE1,"Responded"); Wait(1000); }
実際にこれを使用するときは"koushin.nxc"という名前をつけて保存し
includeを使用して呼び出した。これによりプログラムがすっきりした。
これのおかげで上のフローチャートで示したような、動きが可能になった。
さてそれでは次に缶を2,3,4と積んでいく動きをさせてみる。(因みに4個目を積み終わったら地面に下ろす)
1個目を掴んで持ち上げた後の、2〜4個目を積む動作は同じである。
具体的には2個目以降の缶を見つたら、
缶を載せる→アームを離す→アームを上に→後退→アームを最下部へ→缶を掴む→アームを上に
という動きをひたすら繰り返させれば良い。
そして出来上がったのが次のプログラム。今回から"koushin.nxc"は毎回使用した。
✡マスター側
#define CONN 1 #define SIGNAL_A 11 #define SIGNAL_B 12 #define SIGNAL_C 13 #define SIGNAL_D 14 #define SIGNAL_E 15 #define SIGNAL_F 16 #define SIGNAL_G 17 #define go_forward OnFwd(OUT_AB,50);//出力50%で前進 #include "koushin.nxc" task main() { SetSensorLowspeed(S4); int hakken=0;//発見した回数をカウント while(hakken==0){ go_forward; if(SensorUS(S4)<=12){//もし12センチ以内に缶を発見したら SendRemoteNumber(CONN,MAILBOX1,SIGNAL_A); /*Sのメールボックス1にSIGNAL_Aを送信*/ Off(OUT_AB); /*停止(この間アームが下りて缶をキャッチ、そしてアームを上げる)*/ receive(MAILBOX2,"SIGNAL_C");//Sからメールボック2にSIGNAL_Cを受信 go_forward;Wait(500);Off(OUT_AB);//0.5秒前進 sendmsg(CONN,MAILBOX1,MAILBOX2,"SIGNAL_D","SIGNAL_E"); /*Sのメールボックス1にSIGNAL_Dを送信*/ hakken++; }//if }//while(hakken==0) /*hakkenの値が1となり次のwhile(hakken==1)に移行*/ while(hakken==1,2,3){//hakken=1,2,3の時 TextOut(0,LCD_LINE2,"while(hakken==1,2,3)"); go_forward; if(SensorUS(S4)<=8){//もし8センチ以内に缶を発見したら SendRemoteNumber(CONN,MAILBOX1,SIGNAL_B); /*Sのメールボックス1にSIGNAL_Bを送信*/ Off(OUT_AB);//モータAB停止 receive(MAILBOX2,"SIGNAL_F");//Sからメールボックス2にSIGNAL_Fを受信 OnRev(OUT_AB,50);Wait(1500);//1.5秒後退 Off(OUT_AB);//モータAB停止 SendRemoteNumber(CONN,MAILBOX1,SIGNAL_A); /*Sのメールボックス1にSIGNAL_Aを送信(再び缶を掴み、上部へ)*/ Off(OUT_AB);//モータAB停止 receive(MAILBOX2,"SIGNAL_C");//Sからメールボックス2にSIGNAL_Cを受信 OnFwd(OUT_AB,40);Wait(1000);//1秒前進 Off(OUT_AB);Wait(3000);//3秒停止 sendmsg(CONN,MAILBOX1,MAILBOX2,"SIGNAL_D","SIGNAL_E"); /*Sのメールボックス1にSIGNALDを送信*/ hakken++; }//(if_9) }//while(hakken==1,2,3) /*hakkenの値が4になり、次のwhile(hakken==4)に移行*/ while(hakken==4){//hakken==4の時 TextOut(0,LCD_LINE2,"while(hakken==4):"); Off(OUT_AB);//モータAB停止 SendRemoteNumber(CONN,MAILBOX1,SIGNAL_G); /*Sのメールボックス1にSIGNAL_Gを送信*/ receive(MAILBOX2,"SIGNAL_H"); /*SからSIGNAL_Hを受信(それまで待機)*/ RotateMotorEx(OUT_AB,-40,500,0,true,true);//ちょっと後退(缶にぶつからないように) sendmsg(CONN,MAILBOX1,MAILBOX2,"SIGNAL_I","SIGNAL_J"); /*SのMAILBOX1にSIGNAL_Iを送信*/ /*SからMAILBOX2にSIGNAL_Jが来るまで待機*/ hakken++;//hakkenをカウント(これにより停止) }//while(hakken==2) }//task
✡スレイブ側
#define CONN 1 #define SIGNAL_A 11 #define SIGNAL_B 12 #define SIGNAL_C 13 #define SIGNAL_D 14 #define SIGNAL_E 15 #define SIGNAL_F 16 #define SIGNAL_G 17 #include "koushin.nxc" task main() { while(true){ int msg;//Mから受け取った値を格納する変数 ReceiveRemoteNumber(MAILBOX1,true,msg); /*Mからメールボックス1にmsgを受信*/ if(msg==SIGNAL_A){//msgがSIGNAL_Aなら RotateMotor(OUT_B,65,4300);//アームを下ろす respond(MAILBOX2,"SIGNAL_C");//Mのメールボックス2にSIGNAL_Cを送信 PlaySound(SOUND_UP);//音を鳴らす(UP) receive(MAILBOX1,"SIGNAL_D");//Mからメールボックス1にSIGNAL_Dを受信 PlaySound(SOUND_DOWN);//音を鳴らす(DOWN) Off(OUT_AB);Wait(2000);//2秒待機 RotateMotor(OUT_A,50,-180);//缶を掴む Off(OUT_A);//モータAを停止 RotateMotor(OUT_B,65,-4300);//アームを一番上まで上げる respond(MAILBOX2,"SIGNAL_E");//Mのメールボックス2にSIGNAL_Eを送信 }//if(msg==SIGNAL_A) if(msg==SIGNAL_B){//msgがSIGNAL_Bなら RotateMotor(OUT_B,60,1350);//缶を載せる RotateMotor(OUT_A,30,170);//缶を離す RotateMotor(OUT_B,60,-1350);//アームを一番上まで上げる respond(MAILBOX2,"SIGNAL_F");//Mのメールボックス2にSIGNAL_Fを送信 }//if(msg==SIGNAL_B) if(msg==SIGNAL_G){//msgがSIGNAL_Gなら RotateMotor(OUT_B,60,4300);//アームを動かし、缶を地面まで下ろす RotateMotor(OUT_A,15,170);//缶を離す respond(MAILBOX2,"SIGNAL_H"); /*後退させる*/ receive(MAILBOX1,"SIGNAL_I"); /*MからSIGNAL_Iを受信*/ RotateMotor(OUT_B,50,-4400);//アームを一番上へ上げる respond(MAILBOX2,"SIGNAL_J"); }//if(msg==SIGNAL_G) }//while(true) }//task main
缶が微妙にずれることを除いてはうまくいっているようなので
次のステップに進むとする。細かい調整はそこで行う。
・主にpianoman氏担当
彼が作成した次のようなサブルーチンを使用して、缶を発見している。 尚このプログラムを使用したのは下図の緑色で示した4個の缶を見つける時である。
超音波センサを起動させたまま、機体を回転させ、
周りにある物体と、機体の距離を測らせせる。
その上で、機体からもっとも近い位置にある物体を割り出させる。
これが掴むべき缶である。
↑このように期待を左右に交互に回転させながら、周りにある物体との
距離を測っていく。
↓より詳しいことはこちら↓
2013b/Member/pianoman/Mission2
sub find_direction() {//缶を見つけるサブルーチン int takatyan; int zxc[unko]; int min; int sumofangle; /* ready */ tl; Wait(1000); ResetTachoCount(OUT_AB); Wait(10); /* measure distance between a can and the machine */ for(int i = 0;i <=unko-1;i ++) { tr; Wait(measure_time); zxc[i] = SensorUS(S4); sumofangle += MotorTachoCount(OUT_A); } /* find minimum distance */ min = 40; for(int j = 0;j <=unko-1; j++) { if(min > zxc[j]) { min = zxc[j]; takatyan = j; } } for(int j = 0;j <= unko-1; j++) { if(zxc[j] == 0){zxc[j] = 100; } } float angleR = sumofangle/unko*(unko-takatyan); int angle = trunc(angleR); Off(OUT_AB); TextOut(0,LCD_LINE1,"takatyan ="); NumOut(0,LCD_LINE2,zxc[takatyan]); NumOut(0,LCD_LINE4,angle); Wait(1000); /* turn to right direction */ ResetTachoCount(OUT_AB); tl; until( SensorUS(S4) <= zxc[takatyan] ); Off(OUT_AB); ResetScreen(); NumOut(0,LCD_LINE1,MotorTachoCount(OUT_A)); Wait(100); /* approach a can */ RotateMotor(OUT_A,50,500); Off(OUT_AB); go; until(SensorUS(S4) <= direction); Off(OUT_AB); }
上で紹介した、pianoman氏の缶を見つけるプログラムと、chikuwaの缶を積み上げるプログラムを
合わせたプログラム。スタート地点付近にある3つの缶をまずはこのプログラムで積めるかどうか、
検証してみる。
(なお今回からは、缶の持ち上げ,積み上げ,下ろしの3つの動作はサブルーチン化した)
✡マスター側
#define CONN 1 #define SIGNAL_A 11 #define SIGNAL_B 12 #define SIGNAL_C 13 #define SIGNAL_D 14 #define SIGNAL_E 15 #define SIGNAL_F 16 #define SIGNAL_G 17 #define go_forward OnFwd(OUT_AB,50);//出力50%で前進 #include "koushin.nxc" #define tr OnFwdSync(OUT_AB,60,100); #define tl OnFwdSync(OUT_AB,60,-100); #define go OnFwdSync(OUT_AB,75,0) #define unko 60 #define direction 13 #define measure_time 10
sub can_mochiage() {//缶を持ち上げるサブルーチン SendRemoteNumber(CONN,MAILBOX1,SIGNAL_A); /*Sのメールボックス1にSIGNAL_Aを送信*/ Off(OUT_AB); /*停止(この間にアームが下りる)*/ receive(MAILBOX2,"SIGNAL_C");//Sからメールボック2にSIGNAL_Cを受信 go_forward;Wait(500);Off(OUT_AB);//0.5秒前進(隙間を埋める) sendmsg(CONN,MAILBOX1,MAILBOX2,"SIGNAL_D","SIGNAL_E"); /*Sのメールボックス1にSIGNAL_Dを送信(これにより缶を掴む)*/ /*Sからメールボックス2にSIGNAL_Eが来るまで待機*/ }
sub can_tsumu() {//缶を積み、持ち上げるサブルーチン SendRemoteNumber(CONN,MAILBOX1,SIGNAL_B); /*Sのメールボックス1にSIGNAL_Bを送信(缶を載せ、離し、アームを最上部へ)*/ Off(OUT_AB);//モータAB停止 receive(MAILBOX2,"SIGNAL_F"); /*Sからメールボックス2にSIGNAL_Fを受信*/ OnRev(OUT_AB,50);Wait(1500);//1.5秒後退 Off(OUT_AB);//モータAB停止 SendRemoteNumber(CONN,MAILBOX1,SIGNAL_A); /*Sのメールボックス1にSIGNAL_Aを送信(再び缶を掴み、上部へ)*/ Off(OUT_AB);//モータAB停止 receive(MAILBOX2,"SIGNAL_C"); /*Sからメールボックス2にSIGNAL_Cを受信*/ OnFwd(OUT_AB,40);Wait(1000);//1秒前進 Off(OUT_AB);Wait(3000);//3秒停止 sendmsg(CONN,MAILBOX1,MAILBOX2,"SIGNAL_D","SIGNAL_E"); /*Sのメールボックス1にSIGNALDを送信*/ }
sub can_orosu() {//缶を下ろすサブルーチン SendRemoteNumber(CONN,MAILBOX1,SIGNAL_G); //缶を下ろす receive(MAILBOX2,"SIGNAL_H"); /*SからSIGNAL_Hを受信(それまで待機)*/ RotateMotorEx(OUT_AB,-40,500,0,true,true);//ちょっと後退(缶にぶつからないように) sendmsg(CONN,MAILBOX1,MAILBOX2,"SIGNAL_I","SIGNAL_J"); /*SのMAILBOX1にSIGNAL_Iを送信*/ /*SからMAILBOX2にSIGNAL_Jが来るまで待機*/ }
task main() { SetSensorLowspeed(S4);//センサー4を設定 int hakken=0;//発見した回数をカウントする変数 while(hakken==0){//1個目の缶を探す find_direction();//缶を探して前進 hakken++; }//while(hakken==0) /*hakkenの値が1となり次のwhile(hakken==1)へ移行*/ while(hakken==1){//1個目の缶を持ち上げる can_mochiage();缶を持ち上げる hakken++; }//while(hakken==1) /*hakkenの値が2となり次のwhile(hakken==2)に移行*/ while(hakken==2){//2個目の缶を探す TextOut(0,LCD_LINE2,"while(hakken==2)"); find_direction();//缶を探し前進 hakken++; }//while(hakken==2) /*hakkenの値が3となり次のwhile(hakken==3)へ移行*/ while(hakken==3){//2個目の缶を積む TextOut(0,LCD_LINE2,"while(hakken==3)"); can_tsumu();//缶を積む hakken++; }//while(hakken==3) /*hakkenの値が4となり次のwhile(hakken==4)へ移行*/ while(hakken==4){//3個目の缶を探す TextOut(0,LCD_LINE2,"while(hakken==4)"); find_direction();//缶を探して前進 hakken++; }//while(hakken==4) /*hakkenの値が5となり次のwhile(hakken==5)へ移行*/ while(hakken==5){//3個目の缶を積む TextOut(0,LCD_LINE2,"while(hakken==5)"); can_tsumu();//缶を三個積む hakken++; }//while(hakken==5) }//task
✡スレイブ側
先ほどの「缶を積んでいく」のスレイブ側と全く同じであるので
割愛する。
ちょっと流石に缶から遠すぎたかせいか、缶が見つからないことの方が多かった。
成功すると次のようになる。
↑缶を発見して前進
↑一個目の缶(A)を無難に持ち上げる
↑二個目の缶(B)をフリフリしながら探す
↑二個目の缶の上に缶を置く(あらかじめ、積むのに必要な回転数は計算済み)
↑次は三個目の缶(C)を探してフリフリ
↑三個目の缶もカチッと積む(少し傾いて見えるのはおそらく紙のせい)
↑後退し、倒さない様ゆっくりとアームをおろし、再び持ち上げる。
(前回と同じ動作である)
前述したように、これだと缶が確実に見つからなさそうなので、ところどころに
前進,後退,旋回などをはさみ、次のステップに進んだ。
また本番では、ライントレースをするかどうかも重要なポイントだったので、
実際には2個目の缶を見つけた後、黒線まで後退させ、
そこから、3個目の缶の近くまでライントレースするようなプログラムとなっている。
NXTに文法は合っているのにプログラムが送れないとうい事態が発生!!!
ピアサポートセンターに行って直接先生にお聞きしてみたところ、NXTの容量がいっぱいになっているとご指摘いただきました。
どうせもう使わないだろうし、一応USBにプログラムは入っているのでデータすべて削除!
改めて送信してみると...ピッ。
ちゃんと遅れました。一安心です。(1/20(月))
・主にRedcicudu氏の担当
"ライントレースは概ねしていればよい"とのことであったので、そこにあまり
時間をかけないようにした。
再低減させようと思ったのは、"Bを積んで黒線まで下がった後のカーブ"、
"ゴール直前の蛇行"、"二つのコースのつなぎ目"である。
各項については次のソースコードで少し詳しく述べている。
課題1で使用したのと同じく、交差点を認識したら、変数を一つカウントさせ、
次のステップに移る。(また缶を掴む、積む、置く際にも同様の変数をカウントさせている)
基本はこの動作の繰り返しである。
↓より詳しいことはこちら↓
2013b/Member/Redcicudu/Mission2
//↓ライントレース用の定義↓// #define BB 43 //閾値 #define BW 49 #define WB 55 #define WW 60 #define OnRL(speedR,speedL) OnFwd(OUT_A,speedR);OnFwd(OUT_B,speedL); #define FWD OnRL(35,35); //前進 #define T_LEFT OnRL(40,0); //左折 #define T_RIGHT OnRL(0,40); //右折 #define Q_LEFT OnRL(30,-30); //左旋回 #define Q_RIGHT OnRL(-30,30); //右旋回 #define STEP 5 //一回の判断で行う動作の長さ #define nMAX 20 //カウンタの最大値 #define SB Off(OUT_AB);Wait(500); //小休止 #define cross_line RotateMotorEx(OUT_AB,40,600,0,true,true); //交差点通過 #define cross_line2 RotateMotorEx(OUT_AB,40,300,0,true,true); int Online1=0; //交差点通過ためのカウントの変数 int Online2=0; int hakken=0;
上図の赤色で示した部分を通る際に使用。
(つまり、Cを積んだ後、Dを積んだ後、そしてFを積んだ後
のライントレースに使用している)
sub CROSS_1() //黒線の右の縁に沿って動き、ゴールを目指す。 { while(Online1<nMAX){ if(SENSOR_1<BB){ Q_RIGHT; Online1++; } else { if(SENSOR_1<BW){ T_RIGHT; } else if(SENSOR_1<WB){ FWD; } else if(SENSOR_1<WW){ T_LEFT; } else { Q_LEFT; } Online1=0;//カウンタをリセット }//else Wait(STEP); }//while(nOnline<nMAX) hakken++; //T字路と交差点を通過した回数を追加(それぞれ一回ずつ) SB;//小休止 Online1=0; //カウンタをリセット }
上図の青色で示した部分を通る際に使用。
(つまり、Bを積んだ後黒線まで後退し、Cを積むために直進する
手前のライントレースに使用している)
sub CROSS_2() //三個目の缶を積むためのライントレース { while(Online2<32 50){ if(SENSOR_1<BB){ Q_RIGHT; Online2++; } else { if(SENSOR_1<BW){ T_RIGHT; Online2++; } else if(SENSOR_1<WB){ FWD; Online2++; } else if(SENSOR_1<WW){ T_LEFT; Online2++; } else { Q_LEFT; Online2++; } }//else Wait(STEP); }//while(nOnline2<1000) hakken++; //サブルーチンが終了したらカウント }
上図で示した灰色の部分を通る際に使用。
(つまり前半の4つの缶をゴールに終ろし終わった後、その次のT字路、
そしてHを積んだ後、そのあとのT字路のライントレースに使用している)
sub CROSS_3() //黒線の左の縁に沿って動き、ゴールを目指す。 { while(Online1<nMAX){ if(SENSOR_1<BB){ Q_LEFT; Online1++; } else { if(SENSOR_1<BW){ T_LEFT; } else if(SENSOR_1<WB){ FWD; } else if(SENSOR_1<WW){ T_RIGHT; } else { Q_RIGHT; } Online1=0; } Wait(STEP); } hakken++; //T字路と交差点を通過した回数を追加(それぞれ一回ずつ) SB; cross_line2; Online1=0; //カウンタをリセット }
上図の茶色で示した部分を通る際に使用
(つまり後半でT字路を渡った後、Eを掴むために直進する手前の
ライントレースで使用している)
sub CROSS_4() //黒線の右の縁に沿って動き、ゴールを目指す。 { while(Online2<1000){ if(SENSOR_1<BB){ Q_LEFT; Online2++; } else { if(SENSOR_1<BW){ T_LEFT; Online2++; } else if(SENSOR_1<WB){ FWD; Online2++; } else if(SENSOR_1<WW){ T_RIGHT; Online2++; } else { Q_RIGHT; Online2++; } } Wait(STEP); } hakken++; //T字路と交差点を通過した回数を追加(それぞれ一回ずつ) }
前項で紹介した、
"缶を持ち上げる,積む,下ろす動作"、"缶を見つける動作"、"ライントレース"
をそれぞれサブルーチンを使用し、部品化し、一つのプログラムに統合する。
✡マスター側
各定義文は前項で述べたので割愛する。
各サブルーチンも同様の理由で割愛する。
task main()//task文 { SetSensorLight(S1); //光センサーを設定 SetSensorLowspeed(S4);//センサー4を設定 while(hakken==0){//缶を探す find_direction();//缶を探して前進 hakken++; }//while(hakken==0) while(hakken==1){//一個目の缶を持ち上げる can_mochiage();//一個目の缶を持ち上げる hakken++; }//while(hakken==1) while(hakken==2){//二個目の缶を見つけて前進 migi;//右へ回転 find_direction();//缶を探し前進 RotateMotor(OUT_AB,40,220);//前進 Off(OUT_AB);//モータABを停止 hakken++; }//while(hakken==2) while(hakken==3){//缶を二個積む can_tsumu();//缶を二個積む hakken++; }//while(hakken==3) while(hakken==4){//右へ回転 RotateMotor(OUT_AB,-40,1200);//後退 SB; OnRev(OUT_AB,50); until(SENSOR_1<45); SB; RotateMotor(OUT_AB,-40,200);//後退 SB; choi_migi; SB; hakken++; } while(hakken==5){//あんまり意味ない SB; RotateMotor(OUT_A,-40,10);//ちょっと右後退 SB; hakken++; }//while(hakken==5) while(hakken==6){//適当にライントレース CROSS_2(); }//while(hakken==6) while(hakken==7){//缶を見つけ、積む OnFwdSync(OUT_AB,40,0); //缶を見つけるまで前進 if(SensorUS(S4)<=9){ SB;//小休止 can_tsumu();//缶を積む hakken++; }//if }//while(hakken==7) while(hakken==8){//T字路を渡ったら缶の積み上げに入る RotateMotorEx(OUT_AB,40,600,0,true,true); CROSS_1();//T字路を渡り、hakkenをカウント }//while(hakken==8) while(hakken==9){//缶を見つけ、積む OnFwdSync(OUT_AB,40,0); //缶を見つけるまで前進 if(SensorUS(S4)<=9){ SB;//小休止 can_tsumu();//缶を積む hakken++; }//if }//while(hakken==9) while(hakken==10){ //交差点(ゴール)まで行く CROSS_1();//ライントレース }//while(hakken==10) while(hakken==11){ RotateMotorEx(OUT_AB,40,580,100,true,true); //向きをゴールの方へ向ける SB;//小休止(連続して動作させると変な動きをするので) RotateMotorEx(OUT_AB,40,220,0,true,true); //ゴール内に缶を置けるように前進 can_orosu();//缶を下ろす hakken++; }//while(hakken==11) /*次から後半*/ while(hakken==12){//T字路までライントレース RotateMotorEx(OUT_AB,-40,750,0,true,true);//後退 SB; hoko_tenkan; SB; RotateMotor(OUT_AB,40,350);//前進(誤認識しないように) SB; CROSS_3(); SB; }//while(hakken==12) while(hakken==13){//T字路を曲がる RotateMotorEx(OUT_AB,40,1200,-100,true,true); SB; hakken++; }//while(hakken==13) while(hakken==14){//コースの繋ぎ目を渡る CROSS_3(); SB; }//while(hakken==14) while(hakken==15){//T字路を曲がる RotateMotorEx(OUT_AB,40,1200,-100,true,true);//左旋回 SB; hakken++; }//while(hakken==15) while(hakken==16){//缶の前までライントレース CROSS_4(); SB; }//while(hakken==16) while(hakken==17){//一個目の空き缶を掴む OnFwdSync(OUT_AB,40,0); //缶を見つけるまで前進 if(SensorUS(S4)<=12){ SB; can_mochiage();//缶を一個積む hakken++; }//if }//while(hakken==17) while(hakken==18){//二個目の缶を探す SB; RotateMotor(OUT_A,-50,500); SB; RotateMotor(OUT_AB,-60,2000); SB; RotateMotorEx(OUT_AB,-40,900,100,true,false);//右へ旋回 SB; find_direction();//缶を探し前進 RotateMotor(OUT_AB,40,210);//前進 Off(OUT_AB);//モータABを停止 hakken++;//hakkenをカウント }//while(hakken==18) while(hakken==19){//缶を積む can_tsumu();//缶を二個積む hakken++; }//while(hakken==19) while(hakken==20){//三個目の缶を探し前進 find_direction();//缶を探し前進 hakken++; } while(hakken==21){ OnFwd(OUT_AB,40); if(SensorUS(S4)<=9){ Off(OUT_AB);//モータABを停止 hakken++; } }//while(hakken==21) while(hakken==22){//缶を積む can_tsumu();//缶を三個積む hakken++;//hakkenをカウントする }//while(hakken==22) while(hakken==23){//ライントレースする黒線を探す FWD; if(SENSOR_1<BB){//黒になったら停止 SB; OnFwd(OUT_AB,40); until(SENSOR_1>WW); SB; /*白になったら停止*/ RotateMotor(OUT_AB,40,450);//ちょい前進 SB; RotateMotorEx(OUT_AB,-40,1000,-100,true,false);//左旋回 SB; hakken++;//hakkenをカウント }//if }//while(hakken==23) while(hakken==24){//四個目の缶までライントレース CROSS_1(); }//while(hakken==24) while(hakken==25){//缶を一旦置く SB; RotateMotor(OUT_AB,-40,1000); SB; can_orosu(); SB; can_mochiage(); SB; RotateMotor(OUT_A,40,200); hakken++; /*T字路を渡ったら一旦缶を置く*/ }//while(hakken==25) while(hakken==26){//四個目の缶を積む OnFwdSync(OUT_AB,40,0); //缶を見つけるまで前進 if(SensorUS(S4)<=9){ can_tsumu();//缶を四個積む hakken++; }//if }//while(hakken==26) while(hakken==27){//反転してT字路へ向く RotateMotorEx(OUT_AB,40,2300,100,true,true); hakken++; }//while(hakken==27) while(hakken==28){//T字路間でライントレース CROSS_3(); SB; } //while(hakken==28) while(hakken==29){//T字路を曲がる RotateMotorEx(OUT_AB,40,1400,-100,true,true); SB; hakken++; }//while(hakken==29) while(hakken==30){//コースの繋ぎ目を渡る CROSS_3(); SB; }//while(hakken==30) while(hakken==31){//ゴールへ向く RotateMotorEx(OUT_AB,40,1400,100,true,true); SB; hakken++; }//while(hakken==31) while(hakken==32){//ゴールの前までライントレース CROSS_1(); }//while(hakken==32) while(hakken==33){//缶を置くために微調整 RotateMotorEx(OUT_AB,-40,1650,0,true,true); //缶を倒さないように後退 SB; RotateMotor(OUT_B,40,240);//ちょっとゴールの方へ SB; can_orosu();//缶を下ろす SB; RotateMotorEx(OUT_AB,-40,1500,0,true,true);//缶に触らないように下がる hakken++; }//while(hakken==33) }//task
✡スレイブ側
かなり前の項で紹介した「缶を積んでいく」で紹介したものと同じであるので
ここでは割愛する。
本番前に起こったちょっとしたハプニングを紹介する。
前半,後半あわせてロボットを動かしていると、いつも同じ所で、
ロボットが突然停止するという事態が発生!!
8班に衝撃が走る..!!
と思ったらただ単に、NXTの稼働時間が長すぎて、スリープモードに
入っていただけらしい。
スリープモードを設定で解除して、無事解決!!
一安心。
やはり今まで使っていたコースだと、折り目などのせいで、どうしても缶が傾いてしまう。
そのせいで、いくら缶を丁寧に置いても、ある程度の高さまでくるとどうしても倒れてしまう。
他の班からコースを借りようにも、コースを2つもってきている班はどこにもない。
これは困った...
そこで思いついたのは、"いっそのことコースを新しく作り直してしまおう"という
トンデモ案。本番前日の突貫工事だが、気合で仕上げた。
↑必死に新しいコースを制作中の、Redcicudu氏とchikuwa
本番ももう近いので、部品の付け忘れがないが、あるいはぐらついていないか、
プログラムに穴はないか、最後の点検を行った。
↑最後の最後までチェックを怠らないpianoman氏とRedcicudu氏
リハーサルでは無難にツインタワーを作ることができたので、意気揚々と
スタートさせるも、電池の消費が原因でサブルーチンの"CROSS_1"で白黒を短時間に
数えすぎてしまい、すぐに次の三個目の缶を積む動作に入ってしまい、
結果としてコースアウトしてしまった。
もうダメかと肩を下していると、なんと2巡目があることを知らされた。
そこで"nOnline2"の値を少し増やして、ドキドキ緊張しながら、再び順番が
回ってくるのをまつのであった。
問題の個所はクリア!
やはり、ライントレースの部分は"OnFwd"を使っているので、
電池の残量に作用されやすいようだ。
そして10分間のろのろと時間を費やして...
見事にツインタワー完成!!
缶を最終的に置く位置も何度も修正したかいあって、
何とか円に収まり切ったようだ。
↑最後の最後に見せてくれた見事なツインタワー
(本当にゴールラインギリギリで、下ろし終わるまで心臓バクバクだった...)
プログラムに時間がかかることを見越して、早め早めに行動してきたことが、
結果を出してくれて、非常にうれしく思う。
(それでも、最後は駆け足になってしまったが....)
上のソースコードを見てもらえばわかるように、今回は3人のプログラムが入り乱れて
おり、それぞれ移植するとき色々と手間がかかったがその分だけ
自分たちは一つのチームとして、この課題を達成しようとしているんだな、
ということが実感できた。
またロボット制御の基礎が学ぶことのできるよい機会だった。
今回得た知識をもとにこれからは自分で、アプリ開発などに挑戦したいと思った。