#define THRESHOLD 47 #define SPEED_H 50 //電池あって50 #define SPEED_L 30 //電池あって30 #define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL); #define go_forward OnRL(SPEED_H, SPEED_H); #define turn_left1 OnRL(SPEED_L, -SPEED_L); // 左旋回 #define turn_right1 OnRL(-SPEED_L, SPEED_L); // 右旋回 #define STEP 1 // 1回の判断で動作させる時間 #define nMAX 200 //通常のブとして許容できる繰り返しの最大値 (調整する) #define short_break Off(OUT_BC); Wait(100); // 小休止 #define CROSS_TIME 600 // 交差点通過にかかる時間ライトレースに使用する定義
#define go_forward2 OnFwdSync(OUT_BC,70,0) //早い速度で前進 #define go_forward3 OnFwdSync(OUT_BC,30,0) //遅い速度で前進 #define go_back OnRevSync(OUT_BC,50,0); //後ろに下がる #define short_break2 Off(OUT_BC);Wait(1500); //中休止 #define short_break3 Off(OUT_BC);Wait(3000); //大休止 #define ONRL(speed,angle) RotateMotor(OUT_BC,speed,angle);Off(OUT_BC);//速度と角度指定で前進or後退
移動に使用する定義(特にONRLは今回とても重要な定義である)
#define simeru OnFwd(OUT_A,30); // アームを閉める(紙パックを掴む) #define simeru2 OnFwd(OUT_A,60); // 紙パックを整えるためにアームを閉める #define akeru OnRev(OUT_A,30); //アームを開ける #define akeru2 OnRev(OUT_A,60); //紙パックを整えるためにアームを開ける
アームの開閉に使用する定義
#define CONN 1
通信に使用する定義
sub line(){ SetSensorLight(S3); int nOnline=0; // 続けて黒になった回数 (カウンタ) while (nOnline < nMAX){ if (SENSOR_3 < THRESHOLD-12) { turn_right1; nOnline++; // カウンタを増やす } else { if (SENSOR_3 < THRESHOLD-7) { turn_right1; } else if (SENSOR_3 < THRESHOLD+7) { go_forward; } else if (SENSOR_3 < THRESHOLD+15) { turn_left1; } else { turn_left1; } nOnline=0; // カウンタをリセット } Wait(STEP); } turn_left1; Wait(nMAX*STEP); //進行方向修正 }
ライントレースを行うサブルーチン
sub kabedon(){ SetSensorTouch(S1); SetSensorTouch(S2); go_forward2; until((SENSOR_1 == 1)&&(SENSOR_2 == 1)) }
タッチセンサー2個が反応するまで前進するサブルーチン
sub okiniiku(){ SetSensorTouch(S1); SetSensorTouch(S2); akeru; Wait(2000); // アームを開ける kabedon(); // 1個目をもらうために壁まで前進 simeru; Wait(2000); // 1個目をつかむ(アームを閉める) short_break3; repeat(3){ // 紙パックを整えるためにアームを3回開閉する akeru2; Wait(500); simeru2; Wait(500); } Off(OUT_A); short_break2; RotateMotor(OUT_A,30,-9); // アームを少し閉め直す short_break2; ONRL(30,-430); // 紙パックを自分のフィールドに持ってくるために後退 short_break2; akeru; Wait(2000); // 一旦アームを開ける short_break2; ONRL(30,180); // 位置調整のために少し前進する short_break2; simeru; Wait(2000); // もう一度アームを閉め、紙パックを掴む short_break3; RotateMotor(OUT_C,50,-400); // 90度曲がり、所定の位置の方向に向く short_break3; akeru; Wait(4000); //アームを開く short_break; }
紙パックを受け取り、置きに行くまでのサブルーチン
sub okiniiku2(){ ONRL(50,-195); // 受け取りの位置まで下がる short_break; RotateMotor(OUT_B,50,-430); // 壁に垂直になるように90度曲がる short_break; akeru; Wait(2000); // 受け取りのためにアームを開けておく short_break2; }
紙パックを置いた後、もう一度取りにいけるまでの態勢にし待機するまでのサブルーチン
sub master1(){ SetSensorTouch(S1); int msg=0; // 受け取った数値を格納するための変数 int flag=0; // 処理を終了してwhile文を抜けるための変数 while(flag!=1){ SendRemoteNumber(CONN,MAILBOX1,1); // MAILBOX1に1を送る(ロボット2が渡す準備ができた) ReceiveRemoteNumber(MAILBOX2,true,msg); // MAILBOX2の値を読み込む if(msg==1){ // 読み込んだ数値が1なら(ロボット1が受け取り準備ができていたら)以下の動作を行う go_forward3; // 壁まで前進する until(SENSOR_1 == 1); short_break; akeru; Wait(2000); // アームを開ける short_break; go_back; Wait(1500); // 紙パックをアームで押せる距離まで下がる short_break2; simeru; Wait(500); // アームを閉める short_break3; go_forward3; // 紙パックを渡す until(SENSOR_1 == 1); akeru; Wait(2000); // アームを開ける ONRL(50,-360); //2個目を取りにいくために下がる short_break; do{ SendRemoteNumber(CONN,MAILBOX1,2); // MAILBOX1に2を送る(紙パックを渡し終わったことを通知する) ReceiveRemoteNumber(MAILBOX2,true,msg); // MAILBOX2の数値を読み込む(ロボット1が受け取り終わったかどうか) }while(msg!=2); flag=1; // whileを抜けるための数値をいれる SendRemoteNumber(CONN,MAILBOX1,1); SendRemoteNumber(CONN,MAILBOX2,1); } } }
ロボット1と通信をし前1個の紙パックを渡すサブルーチン
sub master2(){ int msg=0; // 受け取った数値を格納するための変数 int flag=0; // 処理を終了してwhile文を抜けるための変数 while(flag!=1){ SendRemoteNumber(CONN,MAILBOX3,1); // MAILBOX3に1を送る(ロボット2が渡す準備ができた) ReceiveRemoteNumber(MAILBOX4,true,msg); // MAILBOX4の値を読み込む if(msg==1){ do{ SendRemoteNumber(CONN,MAILBOX3,2); // MAILBOX3に2を送る(紙パックを渡し終わったことを通知する) ReceiveRemoteNumber(MAILBOX4,true,msg); // MAILBOX4の数値を読み込む(ロボット1が受け取り終わったかどうか) }while(msg!=2); flag=1; // whileを抜けるための数値をいれる SendRemoteNumber(CONN,MAILBOX3,1); SendRemoteNumber(CONN,MAILBOX4,1); } } }
ロボット1と通信をし後ろ2個の紙パックを渡すサブルーチン
sub master3(){ int msg=0; // 受け取った数値を格納するための変数 int flag=0; // 処理を終了してwhile文を抜けるための変数 while(flag!=1){ SendRemoteNumber(CONN,MAILBOX5,1); // MAILBOX5に1を送る(ロボット2が受け取る準備ができた) ReceiveRemoteNumber(MAILBOX6,true,msg); // MAILBOX6の値を読み込む if(msg==1){ do{ SendRemoteNumber(CONN,MAILBOX5,2); // MAILBOX5に2を送る(紙パックを受け取り終わったことを通知する) ReceiveRemoteNumber(MAILBOX6,true,msg); // MAILBOX6の数値を読み込む(ロボット1に通知されたかどうか) }while(msg!=2); flag=1; // whileを抜けるための数値をいれる SendRemoteNumber(CONN,MAILBOX5,1); SendRemoteNumber(CONN,MAILBOX6,1); } } } sub master4(){ int msg=0; // 受け取った数値を格納するための変数 int flag=0; // 処理を終了してwhile文を抜けるための変数 while(flag!=1){ SendRemoteNumber(CONN,MAILBOX7,1); // MAILBOX7に1を送る(ロボット2が受け取る準備ができた) ReceiveRemoteNumber(MAILBOX8,true,msg); // MAILBOX8の値を読み込む if(msg==1){ do{ SendRemoteNumber(CONN,MAILBOX7,2); // MAILBOX7に2を送る(紙パックを受け取り終わったことを通知する) ReceiveRemoteNumber(MAILBOX8,true,msg); // MAILBOX8の数値を読み込む(ロボット1に通知されたかどうか) }while(msg!=2); flag=1; // whileを抜けるための数値をいれる SendRemoteNumber(CONN,MAILBOX7,1); SendRemoteNumber(CONN,MAILBOX8,1); } } }
ロボット1と通信を行うためのサブルーチン 複数用意されているのは、1つのプログラムをうまく使用できなかったためである。
task main(){ SetSensorLight(S3); SetSensorTouch(S1); SetSensorTouch(S2); ONRL(50,270); // ラインに行くまでの動き RotateMotor(OUT_C,50,420); line(); // ライントレース short_break; RotateMotor(OUT_B,50,-290); // 壁に垂直になるように曲がる short_break; kabedon(); // 壁まで前進する go_back; Wait(900); // 紙パックを取りに行くためにすこし下がる short_break; RotateMotor(OUT_C,50,-357); // 紙パックをとりにくために90度曲がる short_break; ONRL(50,500); // 紙パックの場所まで前進 short_break; simeru; Wait(4000); // 紙パックを掴む short_break; ONRL(50,-250); //渡す位置まで下がる short_break; RotateMotor(OUT_B,50,-342); //壁に垂直になるよう90度曲がる short_break; master1(); /*ここまでが前1個の紙パックを渡すプログラム*/ RotateMotor(OUT_C,50,-357); // 後ろ2個の紙パックを取りに行くために90度曲がる short_break; ONRL(50,670); // 紙パックがある場所まで前進する short_break; simeru; Wait(4000); // 紙パックを掴む short_break3; ONRL(50,-390); //渡す位置まで下がる short_break; RotateMotor(OUT_B,50,-342); // 壁に垂直になるように90度曲がる short_break; Off(OUT_BC); Wait(10000); //ロボット1の邪魔にならないように少し待機 kabedon(); short_break; akeru; Wait(2000); // アームを開ける short_break; ONRL(50,-300); // 位置調整のために少し下がる short_break2; RotateMotor(OUT_A,30,350); // アームを閉める *1 short_break2; go_forward3; // 壁まで前進 until(SENSOR_2 == 1); short_break; akeru; Wait(2000); // アームを開ける go_back; Wait(1000); // 紙パックを受け取る準備のために下がる Off(OUT_BC); master2(); //ここで通信2 /*ここまでが後ろ2個を渡すプログラム*/ master3(); //ここで通信3 Off(OUT_BC); Wait(20000); // ロボット1の邪魔にならないように少し待機 okiniiku(); //2個もっていく ONRL(50,410); short_break; okiniiku2(); /*ここまでが2個の紙パックを所定の位置まで運ぶプログラム*/ master4(); //ここで通信4 Off(OUT_BC); Wait(30000); // ロボット1の邪魔にならないように少し待機 okiniiku(); ONRL(50,450); // 残りの1個を置きにいく short_break; ONRL(50,-400); /*ここまでが残り1個の紙パックを所定の位置まで運ぶプログラム*/ }
初め時間による制御をしていたが電池の残量に大きく左右されていた。しかし、角度による制御法を知ってからかなり正確に制御できるようになり、この課題を上手く進めることができた。しかしながら、それでも電池の残量に少し左右されロボットの制御が難しいということを改めて感じた。 今回はロボットとプログラムの制作両方がとても大変であった。チームのメンバーと何度も集まり試行錯誤しなければならずとても忙しかった。しかし、最終的に紙パックを6個運べ、1位になることができたのでとても嬉しかった。この課題は一人では絶対にできなかったと断言だきる。しかしうまくいったのはチームメンバーの二人の力があったからであり、その力はとても大きいものであったからである。チームメンバーにほんとうに感謝したいと思う。このゼミで機械は所詮は人間が作ったものであるでの間違いや誤差がでてくるのは当然だということ痛感させられ、機械を完璧には信用すべきではないと言うことを学んだ。しかし、その誤差を極力を減らす努力はしていかなければいけないと思った。