目次
詳細はこちら2019a/Mission3
アームはL(L')の玉を直接つかむものとGHIJの玉をプラ容器ごとつかむものの2つとした.
プラ容器内の玉はロボット上部の斜面を伝い反対側に落ちるようにした.
センサを2つ前部に設置し,その2つのセンサ同士の距離もできるだけ狭めることで交差点での停止動作がより正確にできるようにした.また線の左右どちらを辿ろうともセンサを2つ利用することにでロボットがプログラムにかかわらず線の中央に近いところを走ることができる.
まず数値・ロボットが移動するための動きを以下のように定義する
#define step1 38 #define step2 45 #define step3 53 #define step4 60 #define step5 68 #define left0 OnFwd(OUT_A,60);OnRev(OUT_B,60); #define left1 OnFwd(OUT_A,60);Off(OUT_B); #define right0 OnFwd(OUT_B,60);OnRev(OUT_A,90); #define right1 OnFwd(OUT_B,60);Off(OUT_A); #define forward OnFwd(OUT_AB,60); #define back OnRev(OUT_AB,60);
#defineにより定義されたものは以下の通りである.
プログラム名 | 内容 |
step1~5 | 光センサの値域を5つに分けたもの |
left0 | 左タイヤを動かし左折する |
left1 | 両タイヤを動かし左折する |
right0 | 右タイヤを固定し右折する |
right1 | 両タイヤを動かし右折する |
forward | 前進する |
back | 後進する |
sub follow_line_left() { SetSensorLight(S1); SetSensorLight(S2); long t0=0,t1=0,st=1; while(t1-t0<st){ if((SENSOR_1<step1)&&(SENSOR_2<step1)){ //2つのセンサが黒を感知するとき t0=CurrentTick(); forward; //直進する until((SENSOR_1<step3)&&(SENSOR_2<step3));//2つのセンサが灰色を感知するまで t1=CurrentTick(); }else if(SENSOR_1<step1){ left0; }else if(SENSOR_1<step2){ left1; }else if(SENSOR_1<step3){ forward; }else if(SENSOR_1<step4){ right1; }else if(SENSOR_1<step5){ right0; } } Off(OUT_AB); Wait(1000); }
sub follow_line_right() { SetSensorLight(S1); SetSensorLight(S2); long t0=0,t1=0,st=1; while(t1-t0<st){ if((SENSOR_1<step1)&&(SENSOR_2<step1)){ t0=CurrentTick(); forward; until((SENSOR_1<step3)&&(SENSOR_2<step3)); t1=CurrentTick(); }else if(SENSOR_2<step1){ right0; }else if(SENSOR_2<step2){ right1; }else if(SENSOR_2<step3){ forward; }else if(SENSOR_2<step4){ left1; }else if(SENSOR_2<step5){ left0; } } Off(OUT_AB); Wait(1000); }
sub follow_line_back() { SetSensorLight(S1); SetSensorLight(S2); long t0=0,t1=0,st=1; while(t1-t0<st){ if((SENSOR_1<step1)&&(SENSOR_2<step1)){ t0=CurrentTick(); back; until((SENSOR_1<step3)&&(SENSOR_2<step3)); t1=CurrentTick(); }else if(SENSOR_2<step1){ right0; }else if(SENSOR_2<step2){ right1; }else if(SENSOR_2<step3){ back; }else if(SENSOR_2<step4){ left1; }else if(SENSOR_2<step5){ left0; } } Off(OUT_AB); Wait(1000); }
sub start_right() { right1; Wait(100); }
sub start_left() { left1; Wait(100); }
sub across() { SetSensorLight(S1); SetSensorLight(S2); long t0=0,t1=0,st=1; while(t1-t0<st){ if((SENSOR_1<step1)&&(SENSOR_2<step1)){ t0=CurrentTick(); forward; until((SENSOR_1<step3)&&(SENSOR_2<step3)); t1=CurrentTick(); }else if(SENSOR_1<step5){ forward; } } Off(OUT_AB); Wait(1000); }
subによって定義されたプログラムは以下の通り.
プログラム名 | 内容 |
follow_line_left() | 線の左側を線に沿って前進し交差点で停止する |
follow_line_right() | 線の右側を線に沿って前進し交差点で停止する |
sub follow_line_back() | 線の右側を線に沿って後進し交差点で停止する |
start_right() | 右回転する |
start_left() | 左回転する |
across() | 線上で直進し線から外れると停止する |
またロボットがボールを掴み保持したまま移動するためのアーム動作と必要な数値を以下のように定義する.
sub ball_catch1() { OnFwd(OUT_A,40); Wait(200); OnRev(OUT_B,50); Wait(500); OnRev(OUT_A,60); Wait(700); Off(OUT_AB); }
sub ball_throw1() { OnFwd(OUT_A,40); Wait(100); OnFwd(OUT_B,50); Wait(500); OnRev(OUT_A,60); Wait(700); Off(OUT_AB); }
sub ball_catch2() { OnRev(OUT_C,50); Wait(150); Off(OUT_C); }
subで定義したプログラムは以下の通り.
ball_catch1() | 玉を掴み持ち上げる |
ball_throw1() | 玉を下げ置く |
ball_catch2() | プラ容器を掴む |
ロボットがボールを掴み移動するにはロボットが停止したタイミングでアームを動かさなければならない. そのためタイヤを動かすNXTをマスター,アームを動かすNXTをスレーブとして互いにプログラムを終了したことを通信でやり取りできるようにした.
sub send_msg1() //メッセージの送受信(親機) { int msg; while(msg!=SIGNALOFF1){ //SIGNALOFFと受け取るまで SendRemoteNumber(CONN,MAILBOX1,SIGNALON1); //CONNにSIGNALON1を送る ReceiveRemoteNumber(MAILBOX1,true,msg); //メッセージを受け取る } }
sub receive_msg1() //メッセージの送受信と玉を掴むプログラム(子機) { int msg; while(msg!=SIGNALON1){ //SIGNALONと受け取るまで ReceiveRemoteNumber(MAILBOX1,true,msg); //メッセージを受け取る } ball_catch1(); PlaySound(SOUND_UP); SendResponseNumber(MAILBOX1,SIGNALOFF1);//親機にSIGNALOFF1を送る }
receive_msg1()を実行させたのち,親機でfollow_lineの次にsend_msg1()を実行すると親機は線を辿り終えたところで子機にSIGNALON1を送信し,メッセージを受けた子機がball_catch1()を実行し玉を掴む.また掴み終えたところで子機から親機にSIGNALOFF1を送りそのメッセージを受けた親機はsend_msg1()を終了し次のプログラムを実行するようになる.
またPlaySound(SOUND_UP)は通信が正常にできたときアームを動かし終えた合図としてプログラムの確認用に入れてある.
実際に発表実行できたプログラムは以下の通り.
マスター側
#define step1 38 #define step2 45 #define step3 53 #define step4 60 #define step5 68 #define left0 OnFwd(OUT_A,60);OnRev(OUT_B,60); #define left1 OnFwd(OUT_A,60);Off(OUT_B); #define right0 OnFwd(OUT_B,60);OnRev(OUT_A,90); #define right1 OnFwd(OUT_B,60);Off(OUT_A); #define forward OnFwd(OUT_AB,60); #define back OnRev(OUT_AB,60); #define CONN 1 #define SIGNALON1 10 #define SIGNALOFF1 11 #define SIGNALON2 20 #define SIGNALOFF2 21 #define HALF 1900 sub follow_line_left() { SetSensorLight(S1); SetSensorLight(S2); long t0=0,t1=0,st=1; while(t1-t0<st){ if((SENSOR_1<step1)&&(SENSOR_2<step1)){ t0=CurrentTick(); forward; until((SENSOR_1<step3)&&(SENSOR_2<step3)); t1=CurrentTick(); }else if(SENSOR_1<step1){ left0; }else if(SENSOR_1<step2){ left1; }else if(SENSOR_1<step3){ forward; }else if(SENSOR_1<step4){ right1; }else if(SENSOR_1<step5){ right0; } } Off(OUT_AB); Wait(1000); } sub follow_line_right() { SetSensorLight(S1); SetSensorLight(S2); long t0=0,t1=0,st=1; while(t1-t0<st){ if((SENSOR_1<step1)&&(SENSOR_2<step1)){ t0=CurrentTick(); forward; until((SENSOR_1<step3)&&(SENSOR_2<step3)); t1=CurrentTick(); }else if(SENSOR_2<step1){ right0; }else if(SENSOR_2<step2){ right1; }else if(SENSOR_2<step3){ forward; }else if(SENSOR_2<step4){ left1; }else if(SENSOR_2<step5){ left0; } } Off(OUT_AB); Wait(1000); } sub follow_line_back() { SetSensorLight(S1); SetSensorLight(S2); long t0=0,t1=0,st=1; while(t1-t0<st){ if((SENSOR_1<step1)&&(SENSOR_2<step1)){ t0=CurrentTick(); back; until((SENSOR_1<step3)&&(SENSOR_2<step3)); t1=CurrentTick(); }else if(SENSOR_2<step1){ right0; }else if(SENSOR_2<step2){ right1; }else if(SENSOR_2<step3){ back; }else if(SENSOR_2<step4){ left1; }else if(SENSOR_2<step5){ left0; } } Off(OUT_AB); Wait(1000); } sub start_right() { right1; Wait(100); } sub start_left() { left1; Wait(100); } sub across() { SetSensorLight(S1); SetSensorLight(S2); long t0=0,t1=0,st=1; while(t1-t0<st){ if((SENSOR_1<step1)&&(SENSOR_2<step1)){ t0=CurrentTick(); forward; until((SENSOR_1<step3)&&(SENSOR_2<step3)); t1=CurrentTick(); }else if(SENSOR_1<step5){ forward; } } Off(OUT_AB); Wait(1000); } sub send_msg1() { int msg; while(msg!=SIGNALOFF1){ SendRemoteNumber(CONN,MAILBOX1,SIGNALON1); ReceiveRemoteNumber(MAILBOX1,true,msg); } } sub send_msg2() int msg; while(msg!=SIGNALOFF2){ SendRemoteNumber(CONN,MAILBOX2,SIGNALON2); ReceiveRemoteNumber(MAILBOX2,true,msg); } } task main() { follow_line_left(); right_s; Wait(200); forward; Wait(900); Off(OUT_AB); send_msg1(); right_s; Wait(HALF); follow_line_right(); send_msg2() }
スレーブ側
#define CONN 1 #define SIGNALON1 10 #define SIGNALOFF1 11 #define SIGNALON2 20 #define SIGNALOFF2 21 sub ball_catch1() { OnFwd(OUT_A,40); Wait(200); OnRev(OUT_B,50); Wait(500); OnRev(OUT_A,60); Wait(700); Off(OUT_AB); } sub ball_throw1() { OnFwd(OUT_A,40); Wait(100); OnFwd(OUT_B,50); Wait(500); OnRev(OUT_A,60); Wait(700); Off(OUT_AB); } sub ball_catch2() { OnRev(OUT_C,50); Wait(150); Off(OUT_C); } sub receive_msg1() { int msg; while(msg!=SIGNALON1){ ReceiveRemoteNumber(MAILBOX1,true,msg); } ball_catch1(); PlaySound(SOUND_UP); SendResponseNumber(MAILBOX1,SIGNALOFF1); } sub receive_msg2() { int msg; while(msg!=SIGNALON2){ ReceiveRemoteNumber(MAILBOX2,true,msg); } ball_throw1(); PlaySound(SOUND_UP); SendResponseNumber(MAILBOX2,SIGNALOFF2); } task main() { receive_msg1(); receive_msg2(); }
準備・練習の時はうまくいったのに本番うまくいかなかったのは電池の残量やプログラムの確認不足といった準備不足であったと思う.だが線を辿る精度をより高めたり,ロボットが停止してから滑らかに玉を拾う動作に移行できたりと今まで学習した内容を使いこなせていたと思う.