FDIII-HC Starter Kit Guide Chapter9.3 のソース :: Besttechnology

• 差分^[1] を表示
• FDIII-HC Starter Kit Guide Chapter9.3^[2] へ行く。

*プログラミング② [#ucccc301]

プログラミング①では、地面が黒なら右へ、白なら左へを繰り返して進むプログラムでした。このプログラムでは、ずっとジグザグ走行を繰り返すことになり無駄が多いです。

直線ならまっ直ぐ全速力で、カーブはラインに沿って上手く曲がればもっと効率的に進むことができるでしょう。

**赤外線反射センサの値を見る [#bf64d341]

もう一度赤外線反射センサの値を見てみましょう。
以下のプログラムを実行します。

 #include <fd.h>
 #define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}
 
 void main (void) {
   uint8_t ReadData;
 
   fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4);
 
   fd_DXSetEndlessTurn (1, 1);
   fd_DXSetEndlessTurn (2, 1);
 
   while (1) {
     fd_DXReadByteData (100, 28, &ReadData);
     fd_printf ("%3d\r", RightData);
     KEY_QUIT;
   }
 }

#ref(line8.png)

先ず赤外線反射センサが黒い地面の上に来るように置きます。そこからゆっくりと白い地面の方へ移動させます。その時の赤外線反射センサの値を見てください。~
値は黒い地面から白い地面になった時に急に変化するのではなく、徐々に変化して行くのがわかります。

黒と白の丁度真ん中にいる時は全速力で直進、黒の方へ少しずれたら少し右へ、白の方へ大きくずれたら大きく左へというように走行すれば、より効率的に進むことができるかもしれません。

**グラフを書く [#rbd59e5c]

真ん中なら全速力で直進、黒の方へ少しずれたら少し右へ、白の方へ大きくずれたら大きく左へというのをグラフにしてみます。

ここでは、黒い地面上のセンサ値を20、白い地面上のセンサ値を120、黒と白の真ん中にいる時のセンサ値を70としています。必要に応じてグラフを書き換えてください。

#ref(line7.png)

グラフの横軸は赤外線反射センサの値、縦軸は車輪のスピード(符号は除く)です。~
赤いラインが右車輪、青いラインが左車輪です。

黒と白の真ん中(センサ値70)の時、両車輪とも最大速度1000となります(最大値は1023ですが簡略化のため1000としました)。

センサ値が70より小さい時は黒い方へずれているということなので、右車輪のスピードを下げて右へ曲がります。センサ値が小さくなるに従ってスピードを下げ右に曲がる量を増やします。

同様にセンサ値が70より大きい時は白い方へずれているということなので、左車輪のスピードを下げて左へ曲がります。センサ値が大きくなるに従ってスピードを下げ左に曲がる量を増やします。

**式にする [#k98a146a]

グラフの斜めのラインを式にするとどうなるでしょうか。

先ず右車輪の斜めの部分を考えます。
とりあえずグラフを
 y = ax + b
という式で表します。
x=70, y=1000を通るので
 1000 = 70a + b
となります。bをaで表すと
 b = 1000 - 70a
となります。最初の式に代入すると
 y = ax + 1000 - 70a
となります。

aはグラフの傾きです。~
このaを大きくすると、少しでも黒い方へずれたら急峻に右へ旋回し、aを小さくすると黒い方へずれるとゆっくりと右へ旋回することになります。

aを大きくすれば中心に素早く戻るので良いような気がしますが、戻り過ぎて逆へずれてしまい安定しなくなる場合があります。~
aが小さ過ぎればラインのカーブに追従できず、ラインを外れてしまいます。~
aをどのような値にすれば良いかは、実際にライントレースをさせながら決めて行きます。~

ここでは一先ず、センサ値が20の時スピードが0になるようa = 20とします。
 y = 20x - 400

同様に左車輪の式を考えます。
 y = ax + 1000 - 70a
までは同じです。ここでは傾きを-20として
 y = -20x + 2400
とします。

**スピードを求める [#l59804b9]

センサ値が70未満の場合と70以上の場合で条件分けをしてスピードを求めるプログラムを作成します。~
グラフを見ながら考えてください。

先ずセンサ値が70より小さい場合、先程の式でyをspeed、xをReadDataとして
 if (ReadData < 70) {
   speed = (ReadData * 20) - 400;
   fd_DXWriteWordData (1, 32, (1 << 10) + speed);
   fd_DXWriteWordData (2, 32, 1000);
 }
となります。左車輪はスピード=1000で一定です。

しかし、このままではspeedの値がマイナスになってしまう場合があるでしょう。よって
 if (ReadData < 70) {
   if (((ReadData * 20) - 400) < 0) speed = 0;
   else speed = (ReadData * 20) - 400;
   fd_DXWriteWordData (1, 32, (1 << 10) + speed);
   fd_DXWriteWordData (2, 32, 1000);
 }
(ReadData * 20) - 400がマイナスの値ならspeed=0とします。

センサ値70以上の場合も同様に
 else {
   if ((-(ReadData * 20) + 2400) < 0) speed = 0;
   else speed = - (ReadData * 20) + 2400;
   fd_DXWriteWordData (1, 32, (1 << 10) + 1000);
   fd_DXWriteWordData (2, 32, speed);
 }
となります。

**プログラム完成 [#r388ec1a]

 #include <fd.h>
 #define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}
 
 void main (void) {
   uint8_t ReadData;
   uint16_t speed;
 
   fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4);
 
   fd_DXSetEndlessTurn (1, 1);
   fd_DXSetEndlessTurn (2, 1);
 
   while (1) {
     fd_DXReadByteData (100, 28, &ReadData);
     fd_printf ("%3d ", ReadData);
     if (ReadData < 70) {
       if (((ReadData * 20) - 400) < 0) speed = 0;
       else speed = (ReadData * 20) - 400;
       fd_DXWriteWordData (1, 32, (1 << 10) + speed);
       fd_DXWriteWordData (2, 32, 1000);
     } else {
       if ((-(ReadData * 20) + 2400) < 0) speed = 0;
       else speed = - (ReadData * 20) + 2400;
       fd_DXWriteWordData (1, 32, (1 << 10) + 1000);
       fd_DXWriteWordData (2, 32, speed);
  }
     KEY_QUIT;
   }
 }

プログラムを実行してみましょう。~
前章のプログラムと比べて上手くライントレースできるようになったでしょうか？

中心となるセンサ値を変えてみたり、グラフの傾きを変えてみたりしながら、上手くライントレースできる値を探してみてください。


[[次のチャプターへ>FDIII-HC Starter Kit Guide Chapter9.4]]~

[[FDIII-HC Starter Kit Guide]]に戻る

• FDIII-HC Starter Kit Guide Chapter9.3 のバックアップ一覧^[4]
• FDIII-HC Starter Kit Guide Chapter9.3 のバックアップソース(No. All)
  • 1: 2010-10-13 (水) 10:58:25^[5] eid7gud^[6]
  • 2: 2010-10-13 (水) 12:42:22^[7] eid7gud^[6]
  • 現: 2010-10-13 (水) 19:42:08 eid7gud^[6]