アクセス数:今日1 総数&counter([total|today|yesterday]);   最終更新日:2006-02-14 (火) 05:01:33

足(運ぶ)側のプログラム New!

//作成者:SAS
//作成日時:2/6〜7,12
#define go_st OnFwd(OUT_A+OUT_C);             //前に進むマクロ
#define go_ba OnRev(OUT_A+OUT_C);             //後ろに進むマクロ
#define turn_l OnFwd(OUT_C);OnRev(OUT_A);     //左に旋回するマクロ
#define turn_r OnFwd(OUT_A);OnRev(OUT_C);     //右に旋回するマクロ
#define float Float(OUT_A+OUT_C);             //滑らかに止まるマクロ
#define SP(x,y) SetPower(OUT_A,x);SetPower(OUT_C,y);             //A,Cのトルクを変えるマクロ
#define white1  SENSOR_1 >= 46    //センサー1で白は46以上と定義
#define black1 SENSOR_1 <= 45    //センサー1で黒は45以下と定義
#define white3  SENSOR_3 >= 46    //センサー3で白は46以上と定義
#define black3 SENSOR_3 <= 45    //センサー3で黒は45以下と定義

int count ;    //countという変数(今通過した線が何本目かを数える変数)を使う
int r_angled;    //r_amgledという変数(回転を90°にする変数)を使う
int message_upper;    //message_upperという変数(上段の紙パックが何色かを記録する変数)を使う
int message_lower;    //message_lowerという変数(下段の紙パックが何色かを記録する変数)を使う
int times;    //timesという変数(今運んでいる動作が何回目かを数える変数)を使う

task main ()    //
{
   r_angled = 27;                            //直角に曲がる時間
   SetSensor(SENSOR_1, SENSOR_LIGHT);        //センサー1と2を
   SetSensor(SENSOR_3, SENSOR_LIGHT);        //光センサーに定義
   ClearMessage();                           //メッセージを消す
   count = 0;                                //変数countに0を代入
   times = 1;    //変数timesに1を代入
   message_upper = 0;    //変数message_upperに0を代入
   message_lower = 0;    //変数message_lowerに0を代入
   SP(2,2);    //左右のトルクを2にする
   
   start pick_box;    //タスク pick_box を開始する
}

task pick_box ()    //タスク pick_box(箱を掴むタスク)
{
   stop line_trace_back;    //タスク line_trace_back を終了する
   SendMessage(1);Wait(100);    //Message1を送信する
   go_st;Wait(50);float;Wait(150);    //少し前進した後止まり、150待つ    
   ClearTimer(0);    //Timer(0)を0にする
   while(Timer(0) <= 44)    //Timer(0)が44以下のとき
       {go_st;Wait(50);float;Wait(60);    //少し前進して止まり、
       if(Message() == 1)    //Messageが1なら
           {message_upper = 1;}    //message_upperに1を代入
       if(Message() == 2)    //Messageが2なら
           {message_upper = 2;}    //message_upperに2を代入
       if(Message() == 3)    //Messageが3なら
           {message_upper = 3;}    //message_upperに3を代入
       if(Message() == 4)    //Messageが4なら
           {message_lower = Message() - 3;}    //message_lowerにMessageから3を引いたものを代入
       if(Message() == 5)    //Messageが5なら
           {message_lower = Message() - 3;}    //message_lowerにMessageから3を引いたものを代入
       if(Message() == 6)    //Messageが6なら
           {message_lower = Message() - 3;}    //message_lowerにMessageから3を引いたものを代入
       }
   if(Timer(0) >= 45)    //Timer(0)が45以下のとき
       {go_ba;until((black1)&&(black3));}    //左右のセンサーが黒になるまで後退する
   if((black1)&&(black3))      //左のセンサーが黒、右のセンサーが白ならば
       {turn_l;Wait(r_angled);float;}    //90°左に旋回し滑らかに停止する

   if(times <= 2)    //変数 times が2以下のとき
       {repeat(2)    //2回繰り返す
            {go_st;Wait(5);go_st;until((black1)&&(black3));float;}    //少し進んだ後左右のセンサーが黒になるまで前進する
        go_st;Wait(5);float;    //少し進んで滑らかに停止する
        turn_r;Wait(r_angled);float;    //90°右に旋回し滑らかに停止する
        go_st;until((black1)&&(white3));    //左のセンサーが黒、右のセンサーが白になるまで前進する
        turn_l;Wait(r_angled);float;    //90°左に旋回し滑らかに停止する
        go_st;Wait(10);    //少し進む
        start line_trace_for;}    //タスク line_trace_for を開始する
   if(times == 3)    //変数 times が3のとき
            {go_st;Wait(5);go_st;until((black1)&&(black3));float;    //少し進んだ後左右のセンサーが黒になるまで前進する
        go_st;Wait(5);    //少し進む
        turn_r;Wait(r_angled);float;    //90°右に旋回し滑らかに停止する
        go_st;until((black1)&&(white3));    //左のセンサーが黒、右のセンサーが白になるまで前進する
        turn_l;Wait(r_angled);float;    //90°左に旋回し滑らかに停止する
        go_st;Wait(5);    //少し進む
        start line_trace_for;}    //タスク line_trace_for を開始する
   if(times == 4)    //変数 times が2以下のとき
       {repeat(3)    //3回繰り返す
           {go_st;Wait(5);go_st;until((black1)&&(black3));float;}    //少し進んだ後左右のセンサーが黒になるまで前進する
        go_st;Wait(5);    //少し進む
        turn_r;Wait(r_angled);float;    //90°右に旋回し滑らかに停止する
        go_st;until((black1)&&(white3));    //左のセンサーが黒、右のセンサーが白になるまで前進する
        turn_l;Wait(r_angled);float;    //90°左に旋回し滑らかに停止する
        go_st;Wait(5);    //少し進む
        start line_trace_for;}    //タスク line_trace_for を開始する
}

task line_trace_for ()    //タスク line_trace_for(曲がりくねったところをライントレースするプログラム)
{

   stop pick_box;    //タスク pick_box を終了する
   while(true)    //条件が真の間
       {if((white1)&&(white3))      //白いところは
           {SP(1,1);go_st;}         // トルク1で前進する
        if((black1)&&(white3))      //左のセンサーが黒なら
           {SP(3,1);turn_l;}        // 左右のトルクが3:1で左旋回
        if((white1)&&(black3))      //右のセンサーが黒なら
           {SP(1,3);turn_r;}        // 左右のトルクが1:3で右旋回
        if((black1)&&(black3))      //両方 緑 なら
           {SP(7,7);                //左右のトルクを7に戻し、
            while (true)            //真の間繰り返す
               {if((black1)&&(black3))  //左右のセンサーが 緑 なら
                    {count += 1;}                 //変数 count を1増やす

                if(message_lower == count)           //メッセージの番号(message_lower)と変数 count が一致したら、
                    {start put_box;                  //タスク put_box を実行し、
                     stop line_trace_for;}           //タスク line_trace_for を終了する
                if(message_lower != count)           //メッセージの番号(message_lower)と変数 count が一致しなければ、
                    {go_st;}                         //前進する
               }
           }
       }
}

task put_box ()    //タスク put_box(運んできた紙パックの一個目を置くプログラム)
{
   stop line_trace_for;    //タスク line_trace_for を終了する
   stop after_green;stop after_white;stop after_black;    //タスク after_green,after_white,after_black を終了する
   go_st;Wait(10);float;                    //前進した後滑らかに止まる
   turn_r;Wait(r_angled);float;              //90°右に旋回した後滑らかに止まる
   go_ba;Wait(50);                          //後退して機体をカゴに対して真っ直ぐにする
   go_st;Wait(200);                          //前進してカゴにぶつかる

   if((message_lower == 1||2||3)&&(times == 1))    //変数 message_lower が1,2,3の何れか且つ変数 times が1なら
       {SendMessage(2);Wait(200);}    //Message(2)を送る
   if((message_lower == 1||2||3)&&(times >= 2))    //変数 message_lower が1,2,3の何れか且つ変数 times が2なら
       {SendMessage(3);Wait(200);}    //Message(3)を送る
   Wait(300);    //300待つ
   go_ba;until((black1)&&(black3));  //黒い線のところまで後退する

   if(message_lower == 1)    //変数 message_lower が1なら
       {start after_green;}    //タスク after_green を開始する
   if(message_lower == 2)    //変数 message_lower が2なら
       {start after_white;}    //タスク after_white を開始する
   if(message_lower == 3)    //変数 message_lower が3なら
       {start after_black;}    //タスク after_black を開始する
   if(message_lower == 0)    //変数 message_lower が0なら
       {start line_trace_back;}    //タスク line_trace_for を開始する
}

task line_trace_back ()    //タスク line_trace_back(紙パックを掴みに戻るプログラム)
{
   stop put_box;    //タスク put_box を終了する
   times += 1;    //変数 times に1を足す
   go_st;Wait(5);float;                    //少し前進した後滑らかに止まる
   turn_r;Wait(r_angled);float;              //90°右に旋回した後滑らかに止まる

   while(count != 0)                               //条件が真の間繰り返す
       {go_st;until((black1)&&(black3));  //左右のセンサーが黒になるまで前進する
        if((black1)&&(black3));  //左右のセンサーが黒なら、
            {count -= 1;}                    //変数 count を1減らす
        if(count == 0)                   //変数 count が0になったら、
            {go_st;Wait(20);}                  //少し前進する
        }
   while(count == 0)    //変数 count が0 の間繰り返す
       {if((white1)&&(white3))      //白いところは
            {SP(1,1);go_st;}                // トルク1で前進する
        if((black1)&&(white3))      //左のセンサーが黒なら
            {SP(3,1);turn_l;}               // 左右のトルクが3:1で左旋回
        if((white1)&&(black3))      //右のセンサーが黒なら
            {SP(1,3);turn_r;}               // 左右のトルクが1:3で右旋回
        if((black1)&&(black3))      //両方黒なら
            {SP(7,7);count =1;                       //左右のトルクを7に戻し、
        turn_r;Wait(r_angled);float;               //90°右に旋回する
        go_st;until((black1)&&(white3));    //左のセンサーが黒、右のセンサーが白になるまで前進する
        if((black1)&&(white3))    //左のセンサーが黒、右のセンサーが白なら
            {;turn_l;Wait(r_angled);float;    //90°左に旋回する
             go_st;until((black1)&&(black3));count += 1}    //左右のセンサーが黒になるまで前進し、変数 count に1を足す
        }
   if((black1)&&(black3)&&(times <= 2))    //左右のセンサーが黒で且つ変数 times が2以下なら
       {go_st;Wait(5);    //少し進んでから
        go_st;until((black1)&&(black3));float;    //左右のセンサーが黒になるまで前進して滑らかに停止し、
        go_ba;Wait(50);    //少し後退した後
        turn_r;Wait(r_angled);    //90°右に旋回する
        start pick_box;}    //タスク pick_box を開始する
   if((black1)&&(black3)&&(times == 3))    //左右のセンサーが黒で且つ変数 times が3なら
       {go_ba;Wait(50);    //少し後退した後
        turn_r;Wait(r_angled);float;    //90°右に旋回し、
        start pick_box;}    //タスク pick_box を開始する
   if((black1)&&(black3)&&(times == 4))    //左右のセンサーが黒で且つ変数 times が4なら
       {repeat(2)    //2回繰り返す
            {go_st;Wait(5);go_st;until((black1)&&(black3));float;}    //少し進んだ後左右のセンサーが黒になるまで前進する
        go_ba;Wait(50);    //少し後退した後
        turn_r;Wait(r_angled);    //90°右に旋回し
        start pick_box;}}    //タスク pick_box を開始する
}

task after_green ()    //タスク after_green(一個目の紙パックが緑だったとき、二個目の紙パックを置くプログラム)
{
   stop put_box;    //タスク put_box を終了する
   if(message_upper == 2)    //変数 message_upper が2のとき
       {turn_l;Wait(r_angled);float;    //90°左に旋回する
        go_st;until((black1)&&(black3));float;    //左右のセンサーが黒になるまで前進する
        go_st;Wait(30);float;    //少し前進する
        turn_r;Wait(r_angled);float;    //90°右に旋回する
        go_st;Wait(200);message_upper == 0;message_lower ==0;    //200進んで変数 message_upper,message_lowerを0にする
        start put_box;}    //タスク put_box を開始する
   if(message_upper == 3)    //変数 message_upper 
       {turn_l;Wait(r_angled);float;    //90°左に旋回する
        repeat(2)    //2回繰り返す
            {go_st;until((black1)&&(black3));float;}    //左右のセンサーが黒になるまで前進する
        go_st;Wait(30);float;    //少し前進する
        turn_r;Wait(r_angled);float;    //90°右に旋回する
        go_st;Wait(200);message_upper == 0;message_lower ==0;    //200進んで変数 message_upper,message_lowerを0にする
        start put_box;}    //タスク put_box を開始する
}

task after_white ()    //タスク after_green(一個目の紙パックが白だったとき、二個目の紙パックを置くプログラム)
{
   stop put_box;    //タスク put_box を終了する
   if(message_upper == 1)    //変数 message_upper 
       {turn_r;Wait(r_angled);float;    //90°右に旋回する
        go_st;until((black1)&&(black3));float;    //左右のセンサーが黒になるまで前進する
        go_st;Wait(30);float;    //少し前進する
        turn_l;Wait(r_angled);float;    //90°左に旋回する
        go_st;Wait(200);message_upper == 0;message_lower ==0;    //200進んで変数 message_upper,message_lowerを0にする
        start put_box;}    //タスク put_box を開始する
   if(message_upper == 3)    //変数 message_upper 
       {turn_l;Wait(r_angled);float;    //90°左に旋回する
        go_st;until((black1)&&(black3));float;    //左右のセンサーが黒になるまで前進する
        go_st;Wait(30);float;    //少し前進する
        turn_r;Wait(r_angled);float;    //90°右に旋回する
        go_st;Wait(200);message_upper == 0;message_lower ==0;    //200進んで変数 message_upper,message_lowerを0にする
        start put_box;}    //タスク put_box を開始する
}

task after_black ()    //タスク after_green(一個目の紙パッ クが黒だったとき、二個目の紙パックを置くプログラム)
{
   stop put_box;    //タスク put_box を終了する
   if(message_upper == 2)    //変数 message_upper 
       {turn_r;Wait(r_angled);float;    //90°右に旋回する
        go_st;until((black1)&&(black3));float;    //左右のセンサーが黒になるまで前進する
        go_st;Wait(30);float;    //少し前進する
        turn_l;Wait(r_angled);float;    //90°左に旋回する
        go_st;Wait(200);message_upper == 0;message_lower ==0;    //200進んで変数 message_upper,message_lowerを0にする
        start put_box;}    //タスク put_box を開始する
   if(message_upper == 1)    //変数 message_upper 
       {turn_r;Wait(r_angled);float;    //90°右に旋回する
        repeat(2)    //2回繰り返す
        {go_st;until((black1)&&(black3));float;}    //左右のセンサーが黒になるまで前進する
        go_st;Wait(30);float;    //少し前進する
        turn_l;Wait(r_angled);float;    //90°左に旋回する
        go_st;Wait(200);message_upper == 0;message_lower ==0;    //200進んで変数 message_upper,message_lowerを0にする
        start put_box;}    //タスク put_box を開始する
}
  • 工夫した点
    「count」という変数を指定することで、ロボットに今何本目の線を通過したのか数えさせ、目的の色のカゴまで紙パックを運ばせるようにした点。
    「times」と言う変数を指定することで、紙パックを置いて戻ってきたときに、回数によって紙パックを取りに行く場所を変えるようにした点
    2個の紙パックをいっぺんに運ぶということで、「task after〜」というタスクを指定し、なるべく同じプログラムを書かずに行動を多様化させようとした点。
     
  • 苦労した点
    残念だったのが、短時間で書き上げたため、マクロを使ったものの全体としては長ったらしいプログラムとなってしまった点。
     
  • 感想
    これまた先のプログラムと同様に、もう一台のRCXとの連携やロボットの構造により殆ど動くことなく終わってしまったのが物凄く残念だった。
     
    一晩よりさらに短い時間で書いたプログラムなので、もっと良いマクロや他の書き方を使うことによって、さらにすっきり出来たかもしれないのが心残りである。

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2006-02-14 (火) 05:01:33