/* 数字及びしきい値、動作時間を設定する */ #define gostr OnFwd(OUT_A+OUT_C); #define goback OnRev(OUT_A+OUT_C); #define gori OnFwd(OUT_A); OnRev(OUT_C); #define gole OnFwd(OUT_C); OnRev(OUT_A); #define THRESHOLD 40 #define runtimeone 95 #define armtimer 200 #define STEP 300 task main() { SetSensor(SENSOR_1, SENSOR_LIGHT); gostr; ClearTimer(0); ClearTimer(1); /* 黒の部分と白い部分の境界をトレースする */ while(Timer(0) < runtimeone){ if(SENSOR_1 < THRESHOLD){ gori; until(SENSOR_1 > THRESHOLD); gostr; }else{ gole; until(SENSOR_1 < THRESHOLD); gostr; } } /* 一回目の交差点で停止 */ Off(OUT_A+OUT_C); Wait(STEP); gostr; Wait(30); Off(OUT_A+OUT_C); /* 黒の部分と白い部分の境界をトレースする */ while(Timer(1) < armtimer){ if(SENSOR_1 < THRESHOLD){ gori; until(SENSOR_1 > THRESHOLD); gostr; }else{ gole; until(SENSOR_1 < THRESHOLD); gostr; } } /* ボールを掴む動作 */ Off(OUT_A+OUT_C); OnRev(OUT_B); Wait(10); Off(OUT_B); gostr; Wait(30); Off(OUT_A+OUT_C); OnFwd(OUT_B); Wait(10); Off(OUT_B); goback(OUT_A+OUT_C); Wait(40); Off(OUT_A+OUT_C); Wait(30); ClearTimer(0); /* 黒の部分と白い部分の境界をトレースする */ while(Timer(0) < 100){ if(SENSOR_1 < THRESHOLD){ gori; until(SENSOR_1 > THRESHOLD); gostr; }else{ gole; until(SENSOR_1 < THRESHOLD); gostr; } } /* 二回目の交差点で停止 */ Off(OUT_A+OUT_C); Wait(STEP); gostr; Wait(30); Off(OUT_A+OUT_C); ClearTimer(0); /* 黒の部分と白い部分の境界をトレースする */ while(Timer(0) < 190){ if(SENSOR_1 < THRESHOLD){ gori; until(SENSOR_1 > THRESHOLD); gostr; }else{ gole; until(SENSOR_1 < THRESHOLD); gostr; } } /* ボールをシュートする */ Off(OUT_A+OUT_C); Wait(30); gostr; Wait(100); Off(OUT_A+OUT_C); OnRev(OUT_B); Wait(10); Off(OUT_B); goback; Wait(100); Off(OUT_A+OUT_C);; }
#define gostr OnFwd(OUT_A+OUT_C); #define goback OnRev(OUT_A+OUT_C); #define gori OnFwd(OUT_A); OnRev(OUT_C); #define gole OnFwd(OUT_C); OnRev(OUT_A); #define THRESHOLD 40 #define runtimeone 95 #define armtimer 200 #define STEP 300
直進、左折、右折、待機時間を定義する。
while(Timer(0) < runtimeone){ if(SENSOR_1 < THRESHOLD){ gori; until(SENSOR_1 > THRESHOLD); gostr; }else{ gole; until(SENSOR_1 < THRESHOLD); gostr; } }
黒い線をトレースするプログラム。センサーによって黒い部分と白い部分を見分け、その境界を進む。
/* 一回目の交差点で停止 */ Off(OUT_A+OUT_C); Wait(STEP); gostr; Wait(30); Off(OUT_A+OUT_C);
最初の交差点で停止する。タイムを取って停止させたのち(30)だけ直進する。その後はトレースに移行。
/* ボールを掴む動作 */ Off(OUT_A+OUT_C); OnRev(OUT_B); Wait(10); Off(OUT_B); gostr; Wait(30); Off(OUT_A+OUT_C); OnFwd(OUT_B); Wait(10); Off(OUT_B); goback(OUT_A+OUT_C); Wait(40); Off(OUT_A+OUT_C); Wait(30); ClearTimer(0);
車体がボールの位置にくるとトレース作業は一時中断される。その後、前に進んでからアームを動かしてボールを掴み、後退して元の位置に戻った後にトレースを再開する。。
/* ボールをシュートする */ Off(OUT_A+OUT_C); Wait(30); gostr; Wait(100); Off(OUT_A+OUT_C); OnRev(OUT_B); Wait(10); Off(OUT_B); goback; Wait(100); Off(OUT_A+OUT_C);
車体がゴールに近づくとトレースを中断し、ゴールに向かって直進する。ゴールに達するとアームは離され、シュートされる。その後車体はバックしてゴールを脱し、プログラムは終了。
プログラムの完成後5回ほど実験を繰り返したが、殆ど誤差なくゴールできたためこれで完成とした。センサーを使い交差点を認識するプログラムを作りたかったが、時間制御でしか動かすことが出来無かった点が最も大きな反省点である。 とはいえ、前回の文字を書く課題と比べると比較的思い通りにロボットを動かすことが出来た。