![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
int place; //牛乳パックを置く場所を設定
int get=1; //牛乳パックの数
//ライントレース
sub trace() {
while ((SENSOR_1 > 48) || (SENSOR_3 > 48)) {
if (SENSOR_1 < 48) {
OnFwd(OUT_C);
Off(OUT_A);
} else if (SENSOR_3 < 48) {
OnFwd(OUT_A);
Off(OUT_C);
} else {
OnFwd(OUT_A+OUT_C);
}
}
}
//牛乳パックをつかむ
sub getbox() {
Wait(50);
ClearTimer(0);
while (Timer(0) <= 10) {
SendMessage(51);
}
Wait(50);
}
//牛乳パックを置く
sub putbox() {
Wait(50);
ClearTimer(0);
while (Timer(0) <= 10) {
SendMessage(52);
}
Wait(50);
}
task main() {
//センサの設定
SetSensor(SENSOR_1,SENSOR_LIGHT); //ライントレース（左）
SetSensor(SENSOR_2,SENSOR_LIGHT); //牛乳パックの色判別
SetSensor(SENSOR_3,SENSOR_LIGHT); //ライントレース（右）
while(get <= 6) {
//初めの牛乳パック
if (get == 1) {
getbox();
//ライントレースへ
OnRev(OUT_A+OUT_C);
Wait(15);
OnFwd(OUT_C);
Wait(20);
OnFwd(OUT_A);
Wait(100);
until(SENSOR_1 < 45);
//２個目の牛乳パック
} else if (get == 2) {
//つかむ位置合わせ
OnRev(OUT_A+OUT_C);
Wait(40);
Off(OUT_A);
OnFwd(OUT_C);
Wait(40);
OnFwd(OUT_A);
Wait(55);
Off(OUT_C);
Wait(50);
OnFwd(OUT_C);
Wait(30);
Off(OUT_A+OUT_C);
getbox();
//振り返り、そのままライントレースへ
OnRev(OUT_A+OUT_C);
Wait(5);
OnFwd(OUT_A);
Wait(70);
until(SENSOR_1 < 45);
//３個目以降の牛乳パック
} else if (get >= 3) {
//奥から２番目の交差点まで進む
Off(OUT_C);
OnFwd(OUT_A);
Wait(35);
OnFwd(OUT_C);
until(SENSOR_1 < 40);
trace();
OnFwd(OUT_A+OUT_C);
Wait(20);
trace();
//３〜５個目の牛乳パック
if (get <= 5) {
OnFwd(OUT_A+OUT_C);
Wait(20);
Off(OUT_C);
OnRev(OUT_A);
Wait(90);
OnFwd(OUT_A+OUT_C);
//牛乳パックごとに位置を合わせ、牛乳パックをつかむ
if (get == 3) { //３個目
Wait(15);
Off(OUT_A+OUT_C);
getbox();
} else if (get == 4) { //４個目
Wait(50);
Off(OUT_A+OUT_C);
getbox();
OnRev(OUT_A+OUT_C);
Wait(30);
Off(OUT_A+OUT_C);
} else { //５個目
Wait(85);
getbox();
OnRev(OUT_A+OUT_C);
Wait(60);
Off(OUT_A+OUT_C);
}
//黒線上に戻る
OnRev(OUT_A);
until(SENSOR_3 < 45);
OnRev(OUT_C);
Wait(20);
//６個目の牛乳パック
} else {
//一番奥の交差点まで進む
OnFwd(OUT_A+OUT_C);
Wait(20);
trace();
//位置を合わせ、牛乳パックをつかむ
OnFwd(OUT_A+OUT_C);
Wait(20);
Off(OUT_C);
OnRev(OUT_A);
Wait(90);
OnFwd(OUT_A+OUT_C);
Wait(15);
Off(OUT_A+OUT_C);
getbox();
//黒線上へ戻る
OnRev(OUT_A);
until(SENSOR_3 < 45);
OnRev(OUT_C);
Wait(20);
//奥から２番目の交差点まで進む（越える）
trace();
OnFwd(OUT_A+OUT_C);
Wait(20);
}
//ライントレースへ
trace();
OnFwd(OUT_A+OUT_C);
Wait(20);
trace();
Off(OUT_C);
OnFwd(OUT_A);
Wait(25);
OnFwd(OUT_C);
until(SENSOR_1 < 45);
}
//カゴ側へ移動
trace();
//牛乳パックの色判別→入れるカゴを決定
if (SENSOR_2 < 40) { //黒
place = 3;
} else if (SENSOR_2 > 46) { //白
place = 2;
} else { //緑
place = 1;
}
//カゴ前へ移動
repeat(place) {
OnFwd(OUT_A+OUT_C);
Wait(20);
trace();
}
//床上の牛乳パック→持ち上げてカゴに入れる
if ((get >= 3) && (get <= 5)) { //判別（３〜５個目の牛乳パック）
OnFwd(OUT_A+OUT_C);
Wait(30);
Off(OUT_A+OUT_C);
Wait(20);
//車体を上げる
OnFwd(OUT_B);
Wait(100);
Off(OUT_B);
Wait(20);
OnRev(OUT_C);
Wait(100);
OnFwd(OUT_A+OUT_C);
Wait(40);
Off(OUT_A+OUT_C);
putbox();
//牛乳パックを離した後、車体を下げる
OnRev(OUT_B);
Wait(80);
Off(OUT_B);
Wait(20);
//箱上の牛乳パック→そのままカゴに入れる
} else {
OnFwd(OUT_A+OUT_C);
Wait(20);
Off(OUT_A);
OnRev(OUT_C);
Wait(85);
OnFwd(OUT_A+OUT_C);
Wait(30);
Off(OUT_A+OUT_C);
putbox();
}
//黒線上へ戻る
OnRev(OUT_C);
until(SENSOR_1 < 45);
get++; //次に取る牛乳パックの番号を設定
//牛乳パック側へ移動
trace();
repeat(place) {
OnFwd(OUT_A+OUT_C);
Wait(20);
trace();
}
}
}
下段のRCXに組み込んだメインのプログラム。ロボット自体はフィールド上の７個の牛乳パックを運べるよう作っているが、プログラムのほうはは調整が間に合わなかったため、とりあえず６個運ぶところまで書いたもの。下段のRCXには駆動用モータ等、アーム以外の全ての機器が集約されているため、それら全てを制御するこのプログラムは長い。
移動は、牛乳パックをつかむ位置の微調整を除いて、全てライントレースを用いている。ロボット自体が比較的速く動くため、センサの反応がわずかでも、しっかり判定できるよう、黒線検出のセンサ値は高め（トレース時は48以下を黒線上と判定）に設定している。
また、交差点の通過時は、一度センサ判定を止め、0.2秒前進して横線を越えてから再びセンサによるライントレースに戻る方式を採っている。その度にいちいちライントレースの命令文を書くのは面倒なので、ライントレースはサブルーチン（trace()）にまとめ、交差点まで進む命令になっている。
次につかむ牛乳パックが何個目であるかを変数getとし、判別する。初めは1で、牛乳パックを置いたときにプラス１。１つ目を入れたら２つ目を、２つ目を置いたら３つ目を取りに行くという寸法である。
アームの根本につけた光センサによって、変数placeの値を変化させる。この変数は交差点を越える回数で、「移動」の項で書いた命令をrepeat(place){}で繰り返させ、目的とするカゴの前まで移動する。
なお、変数getの値が3〜5のときは、つかんだ牛乳パックは床上に置いてあったものなので、カゴに入れる前に車体を上げ、持ち上げてからカゴの上に移動、カゴに入れるようにしている。
実は発表時、「センサ２は光センサである」という宣言を書き忘れていた。でも動いたということは、なくてもかまわない？
