#define THRESHOLD 45 #define HP 5 #define LP 2 #define STEP 1//一回の判断で待つ時間 #define go_fwd SetPower(OUT_AC,HP);OnFwd(OUT_AC);//前進 #define turn_left2 SetPower(OUT_C,HP);SetPower(OUT_A,LP);/ OnFwd(OUT_C);OnRev(OUT_A);//左旋回 #define turn_right2 SetPower(OUT_A,HP);SetPower(OUT_C,LP);/ OnFwd(OUT_A);OnRev(OUT_C);//右旋回 #define turn_left1 SetPower(OUT_C,HP);OnFwd(OUT_C);//右回り(強) #define turn_right1 SetPower(OUT_A,HP);OnFwd(OUT_A);//左回り(強) #define turn_left0 SetPower(OUT_C,LP);OnFwd(OUT_C);//左回り #define turn_right0 SetPower(OUT_A,LP);OnFwd(OUT_A);//右回り #define short_break Off(OUT_AC);Wait(200); #define time1 1600//スタートから交差点1まで #define time2 300//交差点1の始まりから終わりまで #define time3 500//交差点2の始まりから終わりまで #define time4 900//ゴールまで #define arm_open SetPower(OUT_B,LP);OnRev(OUT_B);Wait(50);Off(OUT_B);//アームを開く #define arm_close SetPower(OUT_B,LP);OnFwd(OUT_B);Wait(50);Off(OUT_B);//アームを閉じる
缶を掴んでから旋回してから前進し元の場所に缶を置きまたライントレースを行うまでのプログラム。アームの開閉の時間が大きすぎるとアームがロボット本体にあたってしまい、小さすぎると右腕と左腕があたってしまうのでそこの調節に苦労した。
sub can_move ()//缶を運ぶ動作 {arm_close;go_fwd;Wait(150);turn_left2;Wait(150);arm_open;turn_right2;Wait(150);}
通常のライントレーサーをするプログラム。細かいカーブにも対応できるように5段階な分けてプログラムを組んだ。基本的にはラインとの右の境界線を走るようにした。
sub linetracer ()//ライントレーサー { if(SENSOR_1 = 0) if(SENSOR_2 < THRESHOLD-7) {turn_left2} else if(SENSOR_2 < THRESHOLD-5) {turn_left0} else if(SENSOR_2 < THRESHOLD+3) {go_fwd} else if(SENSOR_2 < THRESHOLD+5) {turn_right0} else {turn_right2} Wait(STEP); PlaySound(SOUND_UP); }
交差点1を進むためのプログラム。この交差点では左のラインに入りやすいことがわかったので、交差点で曲がらないように右折と直進のみにした。
sub linecross1 ()//交差点1を渡る動作 { if(SENSOR_2 < THRESHOLD+3) {go_fwd} else {turn_right2} Wait(STEP); short_break; PlaySound(SOUND_UP); }
交差点2を進むためのプログラム。交差点1と同様に交差点で曲がらないように左折と直進のみにした
sub linecross2 ()//交差点2を渡る動作 { if(SENSOR_2 < THRESHOLD-7) {turn_left2} else if(SENSOR_2 < THRESHOLD-5) {turn_left0} else if(SENSOR_2 < THRESHOLD+5) {go_fwd} else {turn_right2} Wait(STEP); short_break; PlaySound(SOUND_DOWN); }
task main() {//0 SetSensor(SENSOR_2, SENSOR_LIGHT); SetSensor(SENSOR_1,SENSOR_TOUCH); ClearTimer(0);スタートから交差点1までは通常のライントレーサーを行う。ここまでのコースは角度が小さいカーブがあるが元のプログラムもそれに対応したものになっているため通常のライントレーサーを行う。
while(FastTimer(0) <= time1) { linetracer () }交差点1を進むためのプログラム。
ClearTimer(0); while(FastTimer(0) <= time2) { linecross1 () }交差点2を進むためのプログラム。
ClearTimer(0); while(FastTimer(0) <= time3) { linecross2 () }交差点2を越えて直線に突き当たりそこで缶を処理してゴールまで進むプログラム。
ClearTimer(0); While(FastTimer(0) <= time4) { linetracer (); if (SENSOR_1==1){ can_move () } }//0
#define THRESHOLD 45 #define HP 5 #define LP 2 #define STEP 1//一回の判断で待つ時間 #define go_fwd SetPower(OUT_AC,HP);OnFwd(OUT_AC);//前進 #define back SetPower(OUT_AC,HP);OnFwd(OUT_AC);//後退 #define turn_left2 SetPower(OUT_C,HP);SetPower(OUT_A,LP);OnFwd(OUT_C);OnRev(OUT_A);//左旋回 #define turn_right2 SetPower(OUT_A,HP);SetPower(OUT_C,LP);OnFwd(OUT_A);OnRev(OUT_C);//右旋回 #define turn_left0 SetPower(OUT_C,LP);OnFwd(OUT_C);//左回り #define turn_right0 SetPower(OUT_A,LP);OnFwd(OUT_A);//右回り #define short_break Off(OUT_AC);Wait(200); #define time1 2450//スタートから交差点1まで #define time2 3050//交差点1の始まりから終わりまで #define time3 3700//交差点2の始まりから終わりまで #define time4 5500//交差点2からゴールまで #define arm_open SetPower(OUT_B,LP);OnRev(OUT_B);Wait(50);Off(OUT_B);//アームを開く #define arm_close SetPower(OUT_B,LP);OnFwd(OUT_B);Wait(50);Off(OUT_B);//アームを閉じる #define nMAX 6//カウンターの上限
sub can_move ()//缶を運ぶ動作 {arm_close;go_fwd;Wait(150);turn_left2;Wait(150);arm_open;back;Wait(150)turn_right2;Wait(150);}
sub linetracer ()//ライントレーサー { if(SENSOR_1 == 0)//タッチセンサーが触れない時 if(SENSOR_2 < THRESHOLD-7) {turn_left2} else if(SENSOR_2 < THRESHOLD-5) {turn_left0} else if(SENSOR_2 < THRESHOLD+3) {go_fwd} else if(SENSOR_2 < THRESHOLD+5) {turn_right0} else {turn_right2} Wait(STEP); PlaySound(DOUBLE_UP); }
sub line_cross () { int nOnline=0; //続けて黒になった回数 始めを0とする while (nOnline < nMAX){ //続けて黒になった回数がMAX以内 if (SENSOR_2 < THRESHOLD -5) { turn_right0; nOnline++; } else { if (SENSOR_2 < THRESHOLD -3) {turn_right0} else if (SENSOR_2 < THRESHOLD +4) {go_fwd} else if (SENSOR_2 < THRESHOLD +7) {turn_left0} else {turn_left0} nOnline=0//カウンタをリセット } Wait(STEP); } short_break; PlaySound(SOUND_UP); go_fwd;//交差点を渡る nOnline=0;//カウンタをリセット }
task main() {//0 SetSensor(SENSOR_2, SENSOR_LIGHT); SetSensor(SENSOR_1,SENSOR_TOUCH); ClearTimer(0); int nOnline=0; while(FastTimer(0) <= time1){ linetracer (); } ClearTimer(0); while(FastTimer(0) <= time2){ linecross (); } ClearTimer(0); while(FastTimer(0) <= time3){ linecross (); } ClearTimer(0); While(FastTimer(0) <= time4){ linetracer (); if (SENSOR_1==1){ can_move (); } } Float(OUT_AC);//ゆっくり止まる}//0