#define blackline 35 //黒線の値の定義 int frag; //スイッチの定義 int stage; //どの缶を取るか判断するための値 int start_time; //直線部分の長さを測るための値 int timer; //時間 sub linetrace (){ if ((SENSOR_1 <= blackline) && (SENSOR_3 <= blackline)){ OnRev(OUT_A + OUT_C); Wait(020); frag++; } else{ if ((SENSOR_1 <= blackline) && (SENSOR_3 >= blackline)){ OnFwd (OUT_A);OnRev (OUT_C); } if ((SENSOR_1 >= blackline) && (SENSOR_3 <= blackline)){ OnFwd (OUT_C);OnRev (OUT_A); } if ((SENSOR_1 >= blackline) && (SENSOR_3 >= blackline)){ OnRev (OUT_A + OUT_C); } } } //ライントレースのプログラム sub hold_can (){ OnFwd (OUT_A + OUT_C); Wait (start_time + 50); Off (OUT_A + OUT_C); Wait (020); OnFwd (OUT_B); Wait (100); frag++; } //缶を掴む動作 sub port_can (){ OnRev (OUT_A+OUT_C); Wait (start_time); Off (OUT_A+OUT_C); OnRev (OUT_B); Wait (030); ClearTimer (1); while (FastTimer (1) <= 300){ SendMessage (1); } OnFwd (OUT_A+OUT_C); Wait (040); Off (OUT_A+OUT_C); frag++; } //缶を離す動作 sub go_straight (){ OnFwd (OUT_A + OUT_C); frag++; } //直線移動(アームの付いている側へ) sub go_down (){ OnRev (OUT_A + OUT_C); frag++; } //直線移動(上とは逆方向へ) sub turn_R (){ Off (OUT_A + OUT_C);OnRev (OUT_A+OUT_C);Wait (007);OnFwd (OUT_A);Wait (093);Off (OUT_A+OUT_C); frag++; } //右回転 sub turn_L (){ Off (OUT_A + OUT_C);OnRev (OUT_A+OUT_C);Wait (007);OnFwd (OUT_C);Wait (093);Off (OUT_A+OUT_C); frag++; } //左回転 task main(){ SetSensor (SENSOR_1, SENSOR_LIGHT); SetSensor (SENSOR_3, SENSOR_LIGHT); //センサーを定義 frag=0; stage=1; start_time=0; timer=0; ClearTimer (0); //各種定数とタイマーを設定 while (true){ if (stage == 1){ //一つ目の缶の場合 if (frag == 0||1||3||9){ linetrace (); } //読み込んだ線の本数によってライントレースをするかしないか決定して実行 if (frag == 2){ start_time = FastTimer (0); turn_R (); } //最初のコーナー(右折) if (frag == 4){ ClearTimer (1); linetrace (); } //もう一つのタイマーを定義 if (frag == 5){ timer = FastTimer (1); turn_L (); } //横線同士の間の長さをタイマーの秒数で定義 if (frag == 6){ hold_can (); } //缶を掴む if (frag == 7){ go_down (); } //缶を掴んだまま後退 if (frag == 8){ turn_L (); } //缶を掴んだまま左折 if (frag == 10){ turn_R (); } //右折 if (frag == 11){ go_straight (); } //直進 if (frag == 12){ port_can (); } //メッセージを送り、缶を渡す if (frag == 13){ stage++; frag = 0; //返り値を初期化 } } if (stage >= 3){ } //以降繰り返し(*未完成) } }
線の交わっているところをどうやって読み込むか悩んだ。秒数での定義はなるべく使わないほうがいいと思った。上のプログラムでは左右に90度曲がるつもりで回転を定義しているけれど実際にはそううまくはいかなかった。やはり線をどうやって読み込むか、それに対してどう反応するかが大事だと思った。線毎にいちいち命令を書いた為にごちゃごちゃして解り難いプログラムになってしまった。