#contents
*課題:紙コップを運ぶロボット  [#f729ca33]
**内容 [#f656a6ff]
課題2で作成したコースを使用してC地点,D地点に置かれた紙コップをQ地点の円の中に置き、その数や重ねた個数を競う。

**ルール [#b6eca92d]


*ロボットの説明 [#h1a6f92b]
**ロボット全体 [#f4876158]
#ref(2016a/Member/kurokouchi/Mission3/01.png,50%,01);
マイコンは1つだけを使用しました。

**光センサー [#zda4b7ac]
#ref(2016a/Member/kurokouchi/Mission3/07.png,50%,01);
ライントレース用の光センサーをマイコンの真下に設置しました。(赤丸部分)

これにより、前方に紙コップを置くスペース(青丸部分)を確保することができました。

#ref(2016a/Member/kurokouchi/Mission3/08.png,50%,01);
紙コップ認識用の光センサーです。(赤丸部分)

紙コップの色(白色)を判別します。

**紙コップ回収用アーム [#b574b5fc]

#ref(2016a/Member/kurokouchi/Mission3/02.png,50%,01);
前方の2本の棒(赤丸部分)で紙コップを挟んで回収します。

#ref(2016a/Member/kurokouchi/Mission3/09.png,50%,01);
  ↓
#ref(2016a/Member/kurokouchi/Mission3/06.png,50%,01);

アームを上げて回収済み紙コップのストッパーとしての機能もあります。
基本的にはアームを上げた状態で移動します。

**ロボットの改良 [#yaaaca9a]

#ref(2016a/Member/kurokouchi/Mission3/10.png,50%,01);

アームを回転させる為のギア部分に余計な力が掛からないように空回りするようにしました。

#ref(2016a/Member/kurokouchi/Mission3/11.png,50%,01);
紙コップがアームの奥まで入ってしまうとうまく重ならないことや、光センサーが反応しないことがあったので、奥まで入らないようにした。(赤丸部分)

また、アームの軽量化のためにブロックから棒状の部品に変更した。(青丸部分)

*プログラムの説明 [#s30e530a]

**基本的な定義 [#y54b4306]

***定数の定義 [#mc5e2d5a]

 #define CROSS_TIME 40
 #define BALANCE_TIME 40

- CROSS_TIME:交差点判別時間
~ 黒部分がCROSS_TIMEより続いたら交差点として判別する。
- BALANCE_TIME:姿勢制御用時間
~ 境界部分がBALANCE_TIMEより続いたら機体がまっすぐとして判別する。

***マクロの定義 [#ca9f59fc]

基本動作のマクロです。

 #define roll_left OnRev(OUT_A); OnFwd(OUT_C); //左回転
 #define turn_left OnFwd(OUT_C); Off(OUT_A); //左旋回
 #define go OnFwd(OUT_A); OnFwd(OUT_C); //前進
 #define turn_right OnFwd(OUT_A); Off(OUT_C); //右旋回
 #define roll_right OnFwd(OUT_A); OnRev(OUT_C); //右回転
 #define back OnRev(OUT_A); OnRev(OUT_C); Wait(100); Off(OUT_AC); //後進

交差点判別後のマクロです。

 #define top OnRev(OUT_C); OnFwd(OUT_A); Wait(CROSS_TIME); OnFwd(OUT_AC); Wait(10);
 #define left OnRev(OUT_A); OnFwd(OUT_C); Wait(20);
 #define right OnRev(OUT_C); OnFwd(OUT_A); Wait(80); OnFwd(OUT_AC); Wait(20);

- #define top:前進

#ref(2016a/Member/kurokouchi/Mission2/08.png,100%,01)

角度を戻してそのまま直進する。


- #define left:左折

#ref(2016a/Member/kurokouchi/Mission2/07.png,100%,01)

角度を戻してそのままライントレースさせる。


- #define right:右折

#ref(2016a/Member/kurokouchi/Mission2/09.png,100%,01)

角度を右寄りにして外側の線に機体を移動する。



**ライントレース [#y54b4306]

今回の課題では正確に各地点へ移動するために、左の境界部分のライントレースプログラムを作成した。

***光センサーでの色判別について [#l08c4391]

- 白部分・・・・・光センサーの値:53以降 
- 弱白部分・・・光センサーの値:52以降〜53以下 
- 境界部分・・・光センサーの値:44以降〜52以下 
- 弱黒部分・・・光センサーの値:41以降〜44以下 
- 黒部分・・・・・光センサーの値:41未満 

***タイマーについて [#t320a82e]

- FastTimer(0):交差点判別用のタイマー
~ 光センサーが黒部分の判定をしている時にカウントをして、0.4秒以上続いたら交差点として判別する。また、黒部分以外の時はClearTimer(0)でFastTimer(0)を初期化する。
- FastTimer(1):交差点誤判別防止用のタイマー
~ 小さなカーブを交差点として誤判別しないために指定秒数間交差点判別を行わないようにする。ここで、指定秒数を変数としてl_timeをグローバル変数として定義する。また、ライントレース開始時にClearTimer(1)でFastTimer(1)を初期化する。


***左境界部分のトレース [#d1357496]
 
  sub line_l(){
    int ch=0; //ループ脱出用変数
    ClearTimer(0); //交差点判別用のタイマー初期化
    ClearTimer(1); //交差点誤判別防止用のタイマー初期化
    while(ch==0) //chが0の時はループを続ける
    {
      if(SENSOR_2 > 53) //白部分 右回転
      {
        ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
              roll_right;
      }
      else if(SENSOR_2 > 52) //弱白部分 右旋回
      {
        ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
              turn_right;
      }
      else if(SENSOR_2 > 44) //境界部分 直進
      {
        ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
              go;
      }
      else if(SENSOR_2 > 41) //弱黒部分 左旋回
      {
        ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化
              turn_left;
      }
      else{ //黒部分 左回転
              roll_left;
      }
 
        if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time){  //交差点判別
         Off(OUT_AC); //車輪を止める
         PlaySound(SOUND_UP);  //音を鳴らす
         Wait(100);  //1秒待つ
         ch=1; //ループ脱出の為にchを1にする
       }
       Wait(1); //0.01秒待つ 
    }
  }


**交差点の判別 [#s1bbda60]
上記のライントレース用プログラム内の交差点判別部分について詳しく説明する。

      if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time) //交差点判別
      {  
       Off(OUT_AC); //車輪を止める
       PlaySound(SOUND_UP);  //音を鳴らす
       Wait(100);  //1秒待つ
       ch=1; //ループ脱出の為にchを1にする
     }

交差点は黒部分が長く続いた時に交差点として判別するようにした。そこでタイマーを使い黒部分以外の時は以下の関数でタイマーを初期化するようにした。また交差点と判別したことが分かるように、判別したら音を鳴らすようにした。

  ClearTimer(0);

タイマーがCROSS_TIMEより長い時間が経過した時に交差点として判別する。CROSS_TIMEは40とする。

  if(FastTimer(0)>CROSS_TIME)

しかし、上記のプログラムではカーブを交差点として誤判別してしまう事があるので、それを防ぐためにタイマーをもう一つ追加して次の指定秒数をカウントしなければ交差点と判別しないようにした。

  if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time)

l_timeを上記の指定秒数として、メインプログラムで指定する。


**姿勢制御プログラム [#a7ef2032]
C,D地点に置いた紙コップを正確に取るために、ラインを使い機体の姿勢をまっすぐにするプログラムを作成しました。

***タイマーについて [#uae84032]

- FastTimer(0):姿勢判別用のタイマー
~ 光センサーが境界部分の判定をしている時にカウントをして、タイマーがBALANCE_TIME以上になったら機体の姿勢をまっすぐとして判別する。また、境界部分以外の時はClearTimer(0)でFastTimer(0)を初期化する。

- FastTimer(1):ライントレース時間判別用のタイマー
~ ライントレースをする時間を指定しなければ、姿勢がまっすぐにならない場合ライントレースをし続けてしまい、C,D地点に置いてある紙コップをとることが出来なくなってしまうので追加しました。また、姿勢制御プログラム開始時にClearTimer(1)でFastTimer(1)を初期化する。

***姿勢制御の方法 [#g5f4ea3c]

#ref(2016a/Member/kurokouchi/Mission3/01.gif,nolink,left,around);
姿勢制御プログラムは、交差点判別プログラムを応用して作成しました。

交差点判別プログラムとの違いは、黒部分の判定ではなく境界部分の判定であることです。

また、ラインの長さは限られているので、一定時間が経過しても姿勢がまっすぐにならなければ、一度処理を止めて後退してからまた姿勢制御プログラムを開始するようにしました。

このプログラムにより、C,D地点にますっぐ侵入することが出来るようになりました。
#clear

***左境界部分での姿勢制御 [#la2de7ff]

 sub balance_l(){
    int ch=0; //ループ脱出用変数
    ClearTimer(0); //姿勢判別用のタイマー初期化
    while(ch==0) //chが0の時はループを続ける
    {
          ClearTimer(1); //ライントレース時間判別用のタイマー初期化
          while(FastTimer(1)<200&&ch==0)
	  {
        if(SENSOR_2 > 53) //白部分 右回転
        {
          ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
                roll_right;
        }
        else if(SENSOR_2 > 52) //弱白部分 右旋回
        {
          ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
                turn_right;
        }
        else if(SENSOR_2 > 44) //境界部分 直進
        {
                go;
        }
        else if(SENSOR_2 > 41) //弱黒部分 左旋回
        {
          ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
                turn_left;
        }
        else{ //黒部分 左回転
          ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
                roll_left;
        }
 
          if(FastTimer(0)>BALANCE_TIME) //姿勢判別
              {  
           Off(OUT_AC); //車輪を止める
           PlaySound(SOUND_UP);  //音を鳴らす
           Wait(100);  //1秒待つ
           ch=1; //ループ脱出の為にchを1にする
         }
         Wait(1); //0.01秒待つ 
      }
          if(ch==0)
          {
	    back; //約2秒分の後退
	    back;
          }
    }
  }

***右境界部分での姿勢制御 [#oa1e2b0b]

 sub balance_r(){
    int ch=0; //ループ脱出用変数
    ClearTimer(0); //姿勢判別用のタイマー初期化
    while(ch==0) //chが0の時はループを続ける
    {
          ClearTimer(1); //ライントレース時間判別用のタイマー初期化
          while(FastTimer(1)<200&&ch==0)
	  {
        if(SENSOR_2 > 53) //白部分 左回転
        {
          ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
                roll_left;
        }
        else if(SENSOR_2 > 52) //弱白部分 左旋回
        {
          ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
                turn_left;
        }
        else if(SENSOR_2 > 44) //境界部分 直進
        {
                go;
        }
        else if(SENSOR_2 > 41) //弱黒部分 右旋回
        {
          ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
                turn_right;
        }
        else{ //黒部分 右回転
          ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化
                roll_right;
        }
 
          if(FastTimer(0)>BALANCE_TIME) //姿勢判別
              {  
           Off(OUT_AC); //車輪を止める
           PlaySound(SOUND_UP);  //音を鳴らす
           Wait(100);  //1秒待つ
           ch=1; //ループ脱出の為にchを1にする
         }
         Wait(1); //0.01秒待つ 
      }
          if(ch==0)
          {
	    back; //約2秒分の後退
	    back;
          }
    }
  }

***姿勢判別について [#i9445880]
上記の姿勢制御プログラム内の姿勢判別部分について詳しく説明する。

 if(FastTimer(0)>BALANCE_TIME) //姿勢判別
 {  
   Off(OUT_AC); //車輪を止める
     PlaySound(SOUND_UP);  //音を鳴らす
     Wait(100);  //1秒待つ
     ch=1; //ループ脱出の為にchを1にする
 }

このプログラムでは、境界部分が長く続いた時に機体がますっぐであると判別するようにした。

そこでタイマーを使い境界部分以外の時は以下の関数でタイマーを初期化するようにした。また機体がますっぐであると判別したことが分かるように、判別したら音を鳴らすようにした。

  ClearTimer(0);

タイマーがBALANCE_TIME(0.4秒)より長い時間が経過した時に機体がますっぐであると判別する。

  if(FastTimer(0)>BALANCE_TIME)

また、機体が一定時間(2秒)経過してもまっすぐにならない場合、一度後退してから姿勢制御プログラムを再開する。

  if(ch==0) //姿勢がまっすぐと判別された場合は後退しない。
  {
	back; //約2秒分の後退
	back;
  }


**紙コップ取得について [#o0a89cab]
今回の課題で最も重要である紙コップの取得プログラムです。

***コップ取得方法 [#cd850069]
コップを二つのアームの間に挟んでからアームを上げることによって紙コップを持ち上げて機体の手前に置くことが出来ます。同じ動作で最大8つまでの紙コップを重ねることが出来ます。

#ref(2016a/Member/kurokouchi/Mission3/05.png,50%,01);
光センサーを使い紙コップを認識したら前進を停止します。

紙コップを二つのアームの間に入れなくてはいけないので、上記の姿勢制御プログラムが重要です。



#ref(2016a/Member/kurokouchi/Mission3/02.gif,nolink,left);

***紙コップ取得プログラム [#u348530d]

  sub cup(){
    OnRev(OUT_B); //アームを下す
    Wait(50);
    Off(OUT_B);
 
    OnFwd(OUT_AC); //前進
    while(SENSOR_1<42); //光センサーがコップを認識するまで前進する
    Off(OUT_AC);  //停止
 
    OnFwd(OUT_B); //アームを上げる
    Wait(50);
    Off(OUT_B);
  }

***C地点からD地点への移動 [#mabbd401]

C地点の紙コップを取得後、左回転し右境界部分で姿勢制御を開始する。

#ref(2016a/Member/kurokouchi/Mission3/03.gif,nolink,left,around);
赤い点:光センサー
#clear

***D地点からC地点への移動 [#y048567e]

D地点の紙コップを取得後、後退をし交差点を超えてから左回転し右境界部分で姿勢制御を開始する。

#ref(2016a/Member/kurokouchi/Mission3/04.gif,nolink,left,around);
赤い点:光センサー
#clear

また右回転ではなく左回転をする理由は、左側にはアームだけしかストッパーがなく紙コップが転倒する可能性が上がるたので、左回転にした。


***左回転プログラム [#gf5e92ce]

 sub turn(){
   OnFwd(OUT_C); //左回転
   OnRev(OUT_A);
   Wait(100);  //黒部分から白部分に出るための時間(D地点からC地点への移動時に特に必要)
   while(SENSOR_2>41); //白部分である場合は回転し続ける
   Off(OUT_AC); //停止
 }

光センサーで判別して回転するので、電池の消費による回転不足をなくすことができた。


**Q地点への紙コップの設置 [#v23d196e]

***Q地点での動き [#c03fa70a]
設置場所の輪の中へ紙コップを置くために、最後の交差点を判別したら以下の処理を実行する。

#ref(2016a/Member/kurokouchi/Mission3/05.gif,nolink,left,around);
1:交差点判別後、機体の姿勢を戻さずにそのまま前進する。

2:前進停止後、アームを下して左側のストッパーをなくす。

3:右回転させ、紙コップを外に出す。

4:後進させ、紙コップが機体に触れないようにする。
#clear

***設置プログラム [#zbc8d2e8]

 sub last(){
   OnFwd(OUT_AC); //前進
   Wait(50); //約0.5秒待つ
   Off(OUT_AC);
   OnRev(OUT_B); //アームを下す
   Wait(50); //約0.5秒待つ
   Off(OUT_B);
   OnFwd(OUT_A); //右回転
   OnRev(OUT_C);
   Wait(100); //約1秒待つ
   back; //約2秒間後進する(1秒ではアームが紙コップに触れている時があるため)
     back;
 }



**メインプログラム [#r51b8f1f]

 task main ()
 {
  
   SetSensor(SENSOR_1, SENSOR_LIGHT); //光センサー1の初期設定(紙コップ認識用)
   SetSensor(SENSOR_2, SENSOR_LIGHT); //光センサー2の初期設定(ライントレース用)
  
   OnFwd(OUT_AC); //スタート地点の枠から脱出
   Wait(60);
 
     //C地点までの移動
   line_l(); //左側境界部分をトレース
   left; //P地点を左折
   l_time=400; //次の交差点までの秒数を4秒に設定
   line_l(); //左側境界部分をトレース
   right; //S地点を右折
   balance_l(); //左境界部分での姿勢制御
 
     //紙コップ回収作業
   cup(); //1つ目の紙コップ取得
   back; //後進
 
     //D地点までの移動
   turn(); //左回転
   balance_r(); //右境界部分での姿勢制御
 
     //紙コップ回収作業
   cup(); //2つ目の紙コップ取得
   back; //後進
 
     //C地点までの移動
   turn(); //左回転
   OnFwd(OUT_AC); //前進
   Wait(20);
   balance_r(); //右境界部分での姿勢制御
 
     //紙コップ回収作業
   cup(); //3つ目の紙コップ取得
   back; //後進
 
     //D地点までの移動
   turn(); //左回転
   OnFwd(OUT_AC); //前進
   Wait(20);
   balance_r(); //右境界部分での姿勢制御
 
     //紙コップ回収作業
   cup(); //4つ目の紙コップ取得
   back; //約4秒間後進
     back; 
     back;
     back;
 
   l_time=200; //次の交差点までの秒数を2秒に設定
 
     //左境界部分へ移動
   OnFwd(OUT_C); //左回転
   OnRev(OUT_A);
   Wait(50);
   OnFwd(OUT_AC); //前進
   Wait(30);
 
     //Q地点までの移動
   line_l(); //左側境界部分をトレース
   left; //S地点を左折
   line_l(); //左側境界部分をトレース
   left; //P地点を左折
   line_l(); //左側境界部分をトレース
 
     //紙コップの設置
   last();
 }


*課題結果 [#i72ce040]


#ref(2016a/Member/kurokouchi/Mission2/05.png,50%,01)


*反省・感想 [#af5aefdb]


トップ   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS