2016a/Member/morita/Mission3 の変更点 - ロボティクス入門ゼミ

追加された行はこの色です。
削除された行はこの色です。
2016a/Member/morita/Mission3 へ行く。
2016a/Member/morita/Mission3 の差分を削除
*目次 [#h88d2194]
#contents
*今回の課題について [#r143fba0]
課題2で使用したコースを使い、紙コップを運び･積み重ねるロボットを作成する。詳しいルールなどは、[[課題３:http://yakushi.shinshu-u.ac.jp/robotics/?2016a%2FMission3]]を参照してほしい。

*ロボコンの作戦 [#f748c7c1]
ロボットを2台使い1台は紙コップを運ぶだけ、もう1台は紙コップを積み重ねるというふうにした。紙コップはC地点とD地点に３個ずつ置いてある。運ぶ方のロボットは、コースをB地点→R直進→P直進→S右折→C地点というふうにライントレースをして紙コップを1個取り、再びライントレースでSPの真ん中まで紙コップを運びそこに置き、今度はD地点に紙コップを取りに行きSPの真ん中まで紙コップを運びそこに置く。この運んで置く動作を繰り返させようとした。積み重ねる方のロボットはA地点→P左折→SPの真ん中までをライントレースし、運ばれてきた紙コップを取りそこで右折してQの円の中に入れようとした。しかし、時間が足りなく上記のようなことは完成出来なかったので、ロボコンの前日に3点をとる計画に変えた。実際には、紙コップを運ぶ方のロボットを使いコースをB地点→R直進→P直進→S右折→C地点というふうにライントレースをして紙コップを1個取り、そこで右折をしてQの円内に入れようとした。
*今回作成した機体について [#xee9f107]
**ロボコンで使用したもの・運ぶ方のロボット [#ac07baa2]
前から見た図。アームは固定されていて紙コップを1つずつ運べるようにした。
#ref(2016a/Member/morita/Mission3/IMG_0358.JPG,center,20%,robo)
後ろから見た図。モータを2つ使い前後左右に動かせるようにした。
#ref(2016a/Member/morita/Mission3/IMG_0359.JPG,center,20%,ko-su)
機体の裏側。光センサを機体の中心に設置した。
#ref(2016a/Member/morita/Mission3/IMG_0370.JPG,center,20%,ko-su)

**積み重ねる方のロボット [#ee7013f8]
図のようにギザギザになっている部分と歯車をかみ合わせ、それをモータを使い動かすことにより上下運動を可能にしている。それと連動して上にあがるときにアームが紙コップをつかむ動作をし、下に下がるときに紙コップをはなす動作をする。
#ref(2016a/Member/morita/Mission3/IMG_0366.JPG,left,20%,ko-su)
#ref(2016a/Member/morita/Mission3/IMG_0356.JPG,left,20%,ko-su)
前から見た図↓
#ref(2016a/Member/morita/Mission3/IMG_0367.JPG,left,20%,ko-su)
後ろから見た図。
#ref(2016a/Member/morita/Mission3/IMG_0362.JPG,left,20%,ko-su)
#ref(2016a/Member/morita/Mission3/IMG_0364.JPG,left,20%,ko-su)
#ref(2016a/Member/morita/Mission3/IMG_0365.JPG,left,20%,ko-su)
紙コップをつかんだことを確認出来るように下図のようにタッチセンサを取り付けた。
#ref(2016a/Member/morita/Mission3/IMG_0369.JPG,left,20%,ko-su)
横から見た図↓。光センサはなるべく機体の中心に設置出来るようにした。
#ref(2016a/Member/morita/Mission3/IMG_0352.JPG,left,20%,ko-su)
前から見た図↓。紙コップをつかんだときに滑らないようにアームの先端に車輪を取り付けた。
#ref(2016a/Member/morita/Mission3/IMG_0354.JPG,left,20%,ko-su)
後ろから見た図↓
#ref(2016a/Member/morita/Mission3/IMG_0357.JPG,left,20%,ko-su)

*プログラムについて [#t6e3fcd5]
**実際にロボコンで使ったプログラム [#pde9e585]
***定義 [#hcc0ab09]
 #define turn_right_SENKAI OnFwd(OUT_A); OnRev(OUT_C);   //旋回
 #define turn_right OnFwd(OUT_A); Off(OUT_C);            //片方のみ動かす
 #define turn_left_SENKAI OnFwd(OUT_C); OnRev(OUT_A);    //旋回
 #define turn_left OnFwd(OUT_C); Off(OUT_A);             //片方のみ動かす
 #define go_forward(t) OnFwd(OUT_AC); Wait(t); Off(OUT_AC);
 #define cross_straight_R turn_left; Wait(50);
 #define cross_straight_L turn_right; Wait(50);
 #define cross_right turn_right; Wait(160);
 #define cross_left turn_left; Wait(160);
 
 #define Black 43     //これより小さかったら黒
 #define White 52     //これより大きかったら白
 #define STEP 1       //一回の判断の時間
 #define turn_time 400     //RからR or LからL (180°回転)
 #define turn_time_change 300      //RからL or LからR (90°ぐらい回転)
 
 #define closs_P 1    //closs Pを曲がる許可　・メッセージを区別するためのもの
 #define GO_cupC 2    //Cを取りに行け　・メッセージを区別するためのもの
 #define GO_cupD 3    //Dを取りに行け　・メッセージを区別するためのもの
 #define COME_cupC 4  //Cを持って来い　・メッセージを区別するためのもの
 #define COME_cupD 5  //Dを持って来い　・メッセージを区別するためのもの
***線の右側をライントレースし交差点で停止するためのサブルーチン [#a2eaa8c5]
なお、ライントレースをする仕組みは、[[2016a/Member/morita/Mission2:http://yakushi.shinshu-u.ac.jp/robotics/?2016a%2FMember%2Fmorita%2FMission2]]のときと同じなのでそれをみてほしい。
 sub trace_line_for_cross_R()
 {
    ClearTimer(0);
    while (FastTimer(0) <  32) {
       if (SENSOR_2 <= Black) {            //黒
          turn_right_SENKAI;
       } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
          turn_right;
       } else if (SENSOR_2 > White -2) {   //白に近いグレー
          turn_left;
          ClearTimer(0);
       } else if (SENSOR_2 >= White) {     //白
          turn_left_SENKAI;
          ClearTimer(0);
       } else {                            //グレー
       OnFwd(OUT_AC);
       ClearTimer(0);
       }
       Wait(STEP);
    }
   Off(OUT_AC);
   PlaySound(SOUND_FAST_UP);
   Wait(100);
 }
***線の左側をライントレースし交差点で停止するためのサブルーチン [#vca55111]
 sub trace_line_for_cross_L()
 {
    ClearTimer(0);
    while (FastTimer(0) <  32) {
       if (SENSOR_2 <= Black) {            //黒
          turn_left_SENKAI;
       } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
          turn_left;
       } else if (SENSOR_2 > White -2) {   //白に近いグレー
          turn_right;
          ClearTimer(0);
       } else if (SENSOR_2 >= White) {     //白
          turn_right_SENKAI;
          ClearTimer(0);
       } else {                            //グレー
       OnFwd(OUT_AC);
      ClearTimer(0);
       }
       Wait(STEP);
    }
    Off(OUT_AC);
    PlaySound(SOUND_FAST_UP);
    Wait(100);
 }
***交差点SからSPの真ん中までライントレースをする(線の右側)ためのサブルーチン [#rcc1b72c]
 sub trace_line_to_start_R()
 {
    ClearTimer(0);
    while (FastTimer(0) < 50) {           
       if (SENSOR_2 <= Black) {            //黒
          turn_right_SENKAI;
       } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
          turn_right;
       } else if (SENSOR_2 > White -2) {   //白に近いグレー
          turn_left;
       } else if (SENSOR_2 >= White) {     //白
          turn_left_SENKAI;
       } else {                            //グレー
       OnFwd(OUT_AC);
       }
       Wait(STEP);
    }
 }
***交差点SからSPの真ん中までライントレースをする(線の左側)ためのサブルーチン [#ze49f4b6]
 sub trace_line_to_start_L()
 {
    ClearTimer(0);
    while (FastTimer(0) < 50) {           
       if (SENSOR_2 <= Black) {            //黒
          turn_left_SENKAI;
       } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
          turn_left;
       } else if (SENSOR_2 > White -2) {   //白に近いグレー
          turn_right;
       } else if (SENSOR_2 >= White) {     //白
          turn_right_SENKAI;
       } else {                            //グレー
       OnFwd(OUT_AC);
       }
       Wait(STEP);
    }
 }
***タスクメイン [#qf12bed2]
 task main()
 {
    SetSensor(SENSOR_2, SENSOR_LIGHT);
    ClearMessage();
    int cupC = 0;
    int cupD = 0;
    int n = 0;
 
    go_forward(50);
    repeat (2) {
       trace_line_for_cross_R();
       cross_straight_R;
    }
    trace_line_for_cross_R();
    cross_right;
    trace_line_for_cross_R();
    turn_right;
    Wait(turn_time_change);            //90°旋回(RからLへ)
    while(SENSOR_2 > White) {
       OnFwd(OUT_AC);
    }
    Off(OUT_AC);
    trace_line_for_cross_L();
    cross_left;
    trace_line_to_start_L();               
    turn_left;
    Wait(100);
    while(SENSOR_2 > White) {
       OnFwd(OUT_AC);
    }
    Off(OUT_AC);
 }
**本来計画していた運ぶ方のプログラム [#v6fb3268]
***定義 [#w52cdb7b]
 #define turn_right_SENKAI OnFwd(OUT_A); OnRev(OUT_C);   //旋回
 #define turn_right OnFwd(OUT_A); Off(OUT_C);            //片方のみ動かす
 #define turn_left_SENKAI OnFwd(OUT_C); OnRev(OUT_A);    //旋回
 #define turn_left OnFwd(OUT_C); Off(OUT_A);             //片方のみ動かす
 #define go_forward(t) OnFwd(OUT_AC); Wait(t); Off(OUT_AC);
 #define cross_straight_R turn_left; Wait(50);
 #define cross_straight_L turn_right; Wait(50);
 #define cross_right turn_right; Wait(130);
 #define cross_left turn_left; Wait(130);
 
 #define Black 43     //これより小さかったら黒
 #define White 52     //これより大きかったら白
 #define STEP 1       //一回の判断の時間
 #define turn_time 400     //RからR or LからL (180°回転)
 #define turn_time_change 290      //RからL or LからR (90°ぐらい回転)
 
 #define closs_P 1    //closs Pを曲がる許可
 #define GO_cupC 2    //Cを取りに行け
 #define GO_cupD 3    //Dを取りに行け
 #define COME_cupC 4  //Cを持って来い
 #define COME_cupD 5  //Dを持って来い
 
***線の右側をライントレースし交差点で停止するためのサブルーチン [#we5a37ad]
 sub trace_line_for_cross_R()
 {
    ClearTimer(0);
    while (FastTimer(0) <  36) {
       if (SENSOR_2 <= Black) {            //黒
          turn_right_SENKAI;
       } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
          turn_right;
       } else if (SENSOR_2 > White -2) {   //白に近いグレー
          turn_left;
          ClearTimer(0);
       } else if (SENSOR_2 >= White) {     //白
          turn_left_SENKAI;
          ClearTimer(0);
       } else {                            //グレー
       OnFwd(OUT_AC);
       ClearTimer(0);
       }
       Wait(STEP);
    }
    Off(OUT_AC);
    PlaySound(SOUND_FAST_UP);
    Wait(100);
 }
***線の左側をライントレースし交差点で停止するためのサブルーチン [#k46103d3]
 sub trace_line_for_cross_L()
 {
    ClearTimer(0);
    while (FastTimer(0) <  36) {
       if (SENSOR_2 <= Black) {            //黒
          turn_left_SENKAI;
       } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
          turn_left;
       } else if (SENSOR_2 > White -2) {   //白に近いグレー
          turn_right;
          ClearTimer(0);
       } else if (SENSOR_2 >= White) {     //白
          turn_right_SENKAI;
          ClearTimer(0);
       } else {                            //グレー
       OnFwd(OUT_AC);
       ClearTimer(0);
       }
       Wait(STEP);
    }
    Off(OUT_AC);
    PlaySound(SOUND_FAST_UP);
    Wait(100);
 }
***交差点SからSPの真ん中までライントレースをする(線の右側)ためのサブルーチン [#d4e9c2e3]
 sub trace_line_to_start_R()
 {
    ClearTimer(0);
    while (FastTimer(0) < 50) {           
       if (SENSOR_2 <= Black) {            //黒
          turn_right_SENKAI;
       } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
          turn_right;
       } else if (SENSOR_2 > White -2) {   //白に近いグレー
          turn_left;
       } else if (SENSOR_2 >= White) {     //白
          turn_left_SENKAI;
      } else {                            //グレー
      OnFwd(OUT_AC);
       }
       Wait(STEP);
    }
  }
***交差点SからSPの真ん中までライントレースをする(線の左側)ためのサブルーチン [#z3fe0595]
 sub trace_line_to_start_L()
 {
    ClearTimer(0);
    while (FastTimer(0) < 50) {           
       if (SENSOR_2 <= Black) {            //黒
          turn_left_SENKAI;
       } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
          turn_left;
       } else if (SENSOR_2 > White -2) {   //白に近いグレー
          turn_right;
       } else if (SENSOR_2 >= White) {     //白
          turn_right_SENKAI;
       } else {                            //グレー
       OnFwd(OUT_AC);
       }
       Wait(STEP);
    }
 } 

***紙コップをSPの真ん中にもってくるための動作 [#rce95d34]
 task go_to_CD()
 {
    int cupC = 0;
    int cupD = 0; 
 
    repeat(3){
       int n = 0;
       int m = 0; 
 
       while (n < 1) {
          if(Message() == GO_cupD) {         //C貰ったからDに行ってこい
             PlaySound(SOUND_UP);
             OnRev(OUT_AC);
             Wait(250);                 
             Off(OUT_AC);
             turn_right;
             Wait(turn_time);           //LからLへ
             trace_line_for_cross_L();
             cross_left;
             trace_line_for_cross_L(); 
 
             if(cupD == 0) {
                turn_right;
                Wait(turn_time_change);
                trace_line_for_cross_R();
             }
             if(cupD == 1) {
                go_forward(50);           
                trace_line_for_cross_R();
                turn_right;
                Wait(turn_time);          //180°回転
                trace_line_for_cross_R();
                cross_right;
                trace_line_for_cross_R();
             }
             if(cupD == 2) {
                while (SENSOR_2 > Black) {
                   OnFwd(OUT_AC);
                }
                turn_right;
                Wait(turn_time);
                while (SENSOR_2 > Black) {
                   OnFwd(OUT_AC);
                }
                cross_right;
                trace_line_for_cross_R();
             } 
 
             cupD++;
             while (m < 1) {
                if(Message() == COME_cupD) {      //C置いたからD持って来い
                   PlaySound(SOUND_UP);
                   n++;
                   m++;
                   trace_line_to_start_R();
                } else {
                   Off(OUT_AC);
                }
                Wait(STEP);
              }
          } else {
          Off(OUT_AC);
          }
          Wait(STEP);
       }
 
       while (n < 2) {
          if(Message() == GO_cupC) {         //D貰ったからCに行ってこい
             PlaySound(SOUND_UP);
             OnRev(OUT_AC);
             Wait(250);                 
             Off(OUT_AC);
             turn_left;
             Wait(turn_time);           //RからRへ
             trace_line_for_cross_R();
             cross_right;
             trace_line_for_cross_R(); 
 
             if(cupC == 0) {
                go_forward(50);           
                trace_line_for_cross_L();
                turn_right;
                Wait(turn_time);          //180°回転
                trace_line_for_cross_L();
                cross_left;
                trace_line_for_cross_L();
             }
             if(cupC == 1) {
                while (SENSOR_2 > White) {
                   turn_left;
                }
                while (SENSOR_2 > Black) {
                   OnFwd(OUT_AC);
                }
                turn_right;
                Wait(turn_time);
                while (SENSOR_2 > Black) {
                   OnFwd(OUT_AC);
                }
                cross_left;
                trace_line_for_cross_L();
             } 
 
             cupC++;
             while (m < 2) {
                if(Message() == COME_cupC) {      //D置いたからC持って来い
                   PlaySound(SOUND_UP);
                   n++;
                   m++;
                   trace_line_to_start_L();
                } else {
                   Off(OUT_AC);
                }
                Wait(STEP);
             }
          } else {
          Off(OUT_AC);
          }
          Wait(STEP);
       }
    }
 }
***タスクメイン [#i97b7820]
 task main()
 {
    SetSensor(SENSOR_2, SENSOR_LIGHT);
    ClearMessage();
    int cupC = 0;
    int cupD = 0;
    int n = 0; 
 
    go_forward(50);
    repeat (2) {
       trace_line_for_cross_R();
       cross_straight_R;
    }
    trace_line_for_cross_R();
    cross_right;
    trace_line_for_cross_R();
    turn_right;
    Wait(turn_time_change);            //90°旋回(RからLへ)
    while(SENSOR_2 > White) {
       OnFwd(OUT_AC);
    }
    Off(OUT_AC);
    trace_line_for_cross_L();
    cross_left;
    trace_line_to_start_L();               
    SendMessage(closs_P);                   //用意できたからコッチに来て
    PlaySound(SOUND_CLICK);
    start go_to_CD;
 }
**本来計画していた積み重ねる方のプログラム [#ma8cb089]
***定義 [#ofdcfd02]
 #define turn_right_SENKAI OnFwd(OUT_A); OnRev(OUT_C);   //旋回
 #define turn_right OnFwd(OUT_A); Off(OUT_C);            //片方のみ動かす
 #define turn_left_SENKAI OnFwd(OUT_C); OnRev(OUT_A);    //旋回
 #define turn_left OnFwd(OUT_C); Off(OUT_A);             //片方のみ動かす
 #define go_forward(t) OnFwd(OUT_AC); Wait(t); Off(OUT_AC);
 
 #define Black 43     //これより小さかったら黒
 #define White 47     //これより大きかったら白
 #define STEP 1       //一回の判断の時間
 #define age_sage 350     //上げたり下げたりする時間
 
 #define closs_P 1    //closs Pを曲がる許可
 #define GO_cupC 2    //Cを取りに行け
 #define GO_cupD 3    //Dを取りに行け
 #define COME_cupC 4  //Cを持って来い
 #define COME_cupD 5  //Dを持って来い
***交差点で停止するためのサブルーチン [#lc53460c]
 sub trace_line_for_cross()
 {
   ClearTimer(0);
   while (FastTimer(0) <  36) {
      if (SENSOR_2 <= Black) {            //黒
         turn_right_SENKAI;
      } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
         turn_right;
      } else if (SENSOR_2 > White -2) {   //白に近いグレー
         turn_left;
         ClearTimer(0);
      } else if (SENSOR_2 >= White) {     //白
         turn_left_SENKAI;
         ClearTimer(0);
      } else {                            //グレー
      OnFwd(OUT_AC);
      ClearTimer(0);
      }
      Wait(STEP);
   }
   Off(OUT_AC);
   PlaySound(SOUND_FAST_UP);
   Wait(100);
 }
***永遠にライントレースするためのサブルーチン [#bc732824]
 sub trace_line()
 {
    while (true) {
       if (SENSOR_2 <= Black) {            //黒
          turn_right_SENKAI;
       } else if (SENSOR_2 < Black +2) {   //黒に近いグレー
          turn_right;
       } else if (SENSOR_2 > White -2) {   //白に近いグレー
          turn_left;
       } else if (SENSOR_2 >= White) {     //白
          turn_left_SENKAI;
       } else {                            //グレー
       OnFwd(OUT_AC);
       }
       Wait(STEP);
    }
 }
***交差点Pでメッセージを受け取ったら左折するためのサブルーチン [#j28e9ddb]
 sub crossP()                                       
 {
    ClearTimer(0);
    while (FastTimer(0) <  120 ) {
       if (Message() == closs_P) {
          PlaySound(SOUND_UP);
          turn_left;
       } else {
          Off(OUT_AC);
          ClearTimer(0);
       }
       Wait(STEP);
    }
 } 
***C地点から来たコップを積み重ねるためのサブルーチン [#e0d9b5bd]
cupCという変数は紙コップを積むのが終わったら次の動作に進むための役割をする。（cupDも同じ）
 sub tumutumu_C()
 {
    int cupC = 0;
 
    OnFwd(OUT_B);          //放す＆下げる
    Wait(100);
    Off(OUT_B);
    while (cupC < 1) {
       if (SENSOR_1 == 1) {      //コップ触ってたら
          SendMessage(GO_cupD);      //Dに行ってこい(ただし交差点で待機)
          PlaySound(SOUND_CLICK);
          Wait(200);             //少し待機
          OnRev(OUT_B);          //掴む＆上げる
          Wait(age_sage -100);             
          Off(OUT_B);
          turn_right;            //右向く
          Wait(200);             
          Off(OUT_AC);
          OnFwd(OUT_B);          //放す＆下げる
          Wait(age_sage);
          Off(OUT_B);
          while (SENSOR_2 > Black) {   
             turn_left;
          }
          Off(OUT_AC); 
          SendMessage(COME_cupD);      //さあ持って来い
          PlaySound(SOUND_CLICK);
          cupC++;
       } else {                  //コップ触ってなかったら
       Off(OUT_AC);
       }
       Wait(STEP);
    }
    OnRev(OUT_B);
    Wait(100);
 }
***D地点から来たコップを積み重ねるためのサブルーチン [#w9be2978]
 sub tumutumu_D()
 {
    int cupD = 0; 
 
    OnFwd(OUT_B);          //放す＆下げる
    Wait(100);
    Off(OUT_B);
    while (cupD < 1) {
       if (SENSOR_1 == 1) {      //コップ触ってたら
          SendMessage(GO_cupC);      //Cに行ってこい(ただし交差点で待機)
          PlaySound(SOUND_CLICK);
          Wait(200);             //少し待機
          OnRev(OUT_B);          //掴む＆上げる
          Wait(age_sage -100);             
          Off(OUT_B);
          turn_right;            //右向く
          Wait(200);             
          Off(OUT_AC);
          OnFwd(OUT_B);          //放す＆下げる
          Wait(age_sage);
          Off(OUT_B);
          while (SENSOR_2 > Black) {   
             turn_left;
          }
          Off(OUT_AC); 
          SendMessage(COME_cupC);      //さあ持って来い
          PlaySound(SOUND_CLICK);
          cupD++;
       } else {                  //コップ触ってなかったら
       Off(OUT_AC);
       }
       Wait(STEP);
    }
    OnRev(OUT_B);
    Wait(100);
 }
***タスクメイン [#e763add4]
 task main()
 {
    SetSensor(SENSOR_1, SENSOR_TOUCH);
    SetSensor(SENSOR_2, SENSOR_LIGHT);
    ClearMessage();
    int n = 0;
    int cupC = 0;
    int cupD = 0; 
 
    go_forward(100);
    trace_line_for_cross;
   while (n < 1) {
       if (Message() == closs_P) {
          crossP();
          n++;
       } else {
          Off(OUT_AC);
       }
       Wait(STEP);
    }
    while (n < 2) {
       if (SENSOR_1 == 0) {
          trace_line();
       } else {
          Off(OUT_AC);
          n++;
       }
       Wait(STEP);
    }
    repeat (3) {                      
       tumutumu_C();
       tumutumu_D();
    }
    turn_right;
    Wait(200);
    OnFwd(OUT_AC);
    Wait(400);
    Off(OUT_AC);
 }

*感想・反省 [#b05d0279]
結果としては、紙コップを1つもQの円の中に入れることはできなかった。紙コップを積み重ねるためのロボットをつくるのに多くの時間を費やしてしまったのは反省点だと思う。また、紙コップを積み重ねるためのロボットの機体が大きくなりすぎて小回りがきかなくなってしまったのもうまくできなかった原因だと思う。テスト期間中で忙しい人が多かったけれど、集まれる人で集まって出来たのは良かったと思う。今回の課題では、人間がやれば簡単なことでも機械に同じことをさせるのはすごく大変なことなんだと実感した。授業全体の感想としては、想像していたよりもずっと大変だった。しかし、今まで自分でプログラミングしたロボットを動かすということはしたことがなかったのでとても良い経験になったと思う。