*機構 [#b134b320] *プログラム [#f0357afe] **基本動作の定義付け [#s2d15de5] #define set_power_L SetPower(OUT_A,1);SetPower(OUT_C,1); // 速度を遅く設定する #define THRESHOLD 45 // しきい値の設定 #define go_forward OnFwd(OUT_AC); // 前進 #define turn_right0 OnFwd(OUT_A);OnRev(OUT_C); // 右旋回 #define turn_right1 OnFwd(OUT_A);Off(OUT_C); // 右回転 #define turn_left1 Off(OUT_A);OnFwd(OUT_C); // 左回転 #define turn_left0 OnRev(OUT_A);OnFwd(OUT_C); // 左旋回 #define STEP 1 // 動作を一回行う #define U_turn OnFwd(OUT_A);OnRev(OUT_C);Wait(180);Off(OUT_AC); // Uターンをする **コップを持ち上げるプログラムの定義付け [#m35472aa] #define cup_up OnFwd(OUT_B);Wait(130);Off(OUT_B); // コップを持ち上げる #define cup_down OnRev(OUT_B);Wait(130);Off(OUT_B); // コップを下す **変数 [#j93408b5] int number=0; // グローバル変数。コップの色に応じて1〜3の値をとり、点Aからのパターン分岐を決める **サブルーチン [#d9539c94] ***ライントレース [#nf4e7543] /*論理積を使ったライントレースのサブルーチン*/ sub line(){ if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // センサーが2つともしきい値よりも大きい値をとった時、前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1がしきい値よりも大きい値、センサー3がしきい値よりも小さい値をとった時、右回転をする }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1がしきい値よりも小さい値、センサー3がしきい値よりも大きい値をとった時、左回転をする }else{go_forward; Wait(STEP); } } ***紙コップを掴んだ後の動作 [#o7288d92] /*紙コップを掴んだ後に「点A」に辿り着いてから行う操作のサブルーチン*/ sub carry_cup(){ if(number==1){ while((SENSOR_2<47)&&(SENSOR_2>35)){ if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } go_forward; Wait(10);//紙コップに完全に接触するまで前進。そこからの位置調整も要調査 Off(OUT_AC); cup_down; U_turn; while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップBかCを認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } }else if(number==2){ while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){//交差点を認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } turn_right0; Wait(75);//コップ2の逆方向を向かせる go_forward; Wait(10); U_turn;//交差点でそのまま左折するとコップ2を押してしまう為 while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップ2を認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } go_forward; Wait(20);//紙コップに完全に接触するまで前進。そこからの位置調整も要調査 Off(OUT_AC); cup_down; OnRev(OUT_AC); Wait(130); while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){//交差点を認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } turn_left0; Wait(75);//軸にそって紙コップBかCに向かせる while((SENSOR_2>35)&&(SENSOR_2<47)){//紙コップを認識するまで if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } }else{ while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } go_forward; Wait(10); while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進させる go_forward; } turn_right0; Wait(75); while((SENSOR_2>35)&&(SENSOR_2<47)){//紙コップを認識するまで go_forward; } go_forward; Wait(25); Off(OUT_AC); cup_down; OnRev(OUT_AC); Wait(110); while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進させる OnRev(OUT_AC); } turn_right0; Wait(75); } while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップを認識するまで if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } } /*開始直後の紙コップAを掴み振り向くまでの動きをインライン関数化*/ void start_dash(){ go_forward; Wait(120);//スタート地点からでる為の動き while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){//交差点を認識するまで前進 go_forward; } turn_right0; Wait(75); while((SENSOR_2<47)&&(SENSOR_2>35)){//紙コップを認識するまで if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } go_forward; Wait(100);//コップに接触するまで Off(OUT_AC); if(SENSOR_2>47){//白 number=1; }else if((SENSOR_2<47)&&(SENSOR_2>37)){ number=2; }else{ number=3; }//センサーの値に応じてnumberに値を入れる cup_up; U_turn; } task main(){ SetSensor(SENSOR_1,SENSOR_LIGHT); SetSensor(SENSOR_2,SENSOR_LIGHT); SetSensor(SENSOR_3,SENSOR_LIGHT); set_power_L; int cross=0;//交差点通過回数のカウンタ start_dash();//コップAを掴む carry_cup(); go_forward; Wait(50);//コップBに接触するまで Off(OUT_AC); if(SENSOR_2>47){ number=1; }else if((SENSOR_2>35)&&(SENSOR_2<47)){ number=2; }else{ number=3; }//センサーの値に応じてnumberに値を入れる cup_up; U_turn; ClearTimer(0);//タイマー設置 while(Timer(0)<40){ if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } carry_cup(); go_forward; Wait(50);//コップCに接触するまで Off(OUT_AC); if(SENSOR_2>47){ number=1; }else if((SENSOR_2>35)&&(SENSOR_2<47)){ number=2; }else{ number=3; }//センサーの値に応じてnumberに値を入れる cup_up; U_turn; ClearTimer(0); while(Timer(0)<20){ if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); }else{ go_forward; Wait(STEP); } } carry_cup(); Off(OUT_AC); }