紙パックを運搬するロボットのプログラム New!

//作成者:SAS
//作成日時:2/4

最終更新日:2006-02-14 (火) 04:58:54

#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 SPA(t) SetPower(OUT_A,t);             //Aのトルクを変えるマクロ
#define SPC(t) SetPower(OUT_C,t);             //Cのトルクを変えるマクロ
#define white1  SENSOR_1 >= 42
#define green1 (SENSOR_1 >= 33)&&(SENSOR_1 <= 41)
#define black1 SENSOR_1 <= 32
#define white3  SENSOR_3 >= 42
#define green3 (SENSOR_3 >= 33)&&(SENSOR_3 <= 41)
#define black3 SENSOR_3 <= 32
 
int count ;                                   //countという変数を使う
int r_angled;

task main ()
{
   r_angled = 86;                            //直角に曲がる時間
   SetSensor(SENSOR_1, SENSOR_LIGHT);        //センサー1と2を
   SetSensor(SENSOR_3, SENSOR_LIGHT);        //光センサーに定義
   ClearMessage();                           //メッセージを消す
   count = 0;                                //countに0を代入
   SPA(2);SPC(2);
   
   Wait(1000);
   SendMessage(1);until(Message() !=0)
   if(Message() !=0)
       {SendMessage(2);Wait(400);
        turn_r;                                   //右に旋回する
        until(black1&&white3);}  //左のセンサーが黒、右のセンサーが白になるまで
   if(black1&&white3)      //左のセンサーが黒、右のセンサーが白ならば
       {start line_trace_for;}               //タスク line_trace_for を実行する
}

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

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

task line_trace_back ()
{
   if(white1&&white3)      //白いところは
       {SPA(1);SPC(1);go_st;}                // トルク1で前進する
   if(black1&&white3)      //左のセンサーが黒なら
       {SPA(3);SPC(1);turn_l;}               // 左右のトルクが3:1で左旋回
   if(white1&&black3)      //右のセンサーが黒なら
       {SPA(1);SPC(3);turn_r;}               // 左右のトルクが1:3で右旋回
   if(green1&&green3)      //両方 緑 なら
       {SPA(7);SPC(7);                       //左右のトルクを7に戻し、
        turn_r;Wait(r_angled);               //90°右に旋回する
        stop line_trace_back;}               //タスク line_trace_back を終了する
}
 
task put_box ()
{
   go_st;Wait(30);float;                    //前進した後滑らかに止まる
   turn_r;Wait(r_angled);float;              //90°右に旋回した後滑らかに止まる
   go_ba;Wait(40);                          //後退して機体を真っ直ぐにする
   go_st;Wait(60);                          //前進してカゴにぶつかる
   OnFwd(OUT_B);Wait(100);                  //箱を置く
   OnRev(OUT_B);Wait(100);                  //装置を元に戻す
   go_ba;until(black1&&black3);  //黒い線のところまで後退する
   go_st;Wait(10);float;                    //少し前進した後滑らかに止まる
   turn_r;Wait(r_angled);float;              //90°右に旋回した後滑らかに止まる

   while(true)                               //真の間繰り返す
       {
        go_st;until(green1&&green3);  //左右のセンサーが 緑 になるまで前進する
        if(green1&&green3);  //左右のセンサーが 緑 なら、
            {count -= 1;}                    //変数 count を1減らす
        if(count == 0)                       //変数 count が0になったら、
            {go_st;Wait(10);                //少し前進して、
             start line_trace_back;          //タスク line_trace_back を実行し、
             stop put_box;}                  //タスク put_box を終了する
        else                                 //変数 count が0でなければ、
            {go_st;until(green1&&green3);}  //左右のセンサーが 緑 になるまで前進する
       }
} 
  • 工夫した点
    「count」という変数を指定することで、ロボットに今何本目の線を通過したのか数えさせ、目的の色のカゴまで紙パックを運ばせるようにした点。
     
  • 苦労した点
    苦労した点、と言うか残念だったのが、一度も動くことなくロボットの構想が変わり、どんな動きをするのか確認が出来なかった点。
     
  • 感想
    結局一度も動くことは無かったが、動きの原理はそのまま次のロボットへと引き継がれているので、無駄にはならなくて良かったと思う。
     
    一晩で書いたプログラムなので、もっとマクロや他の書き方を使うことによって、さらにすっきり出来たかもしれないのが心残りである。

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