簡単なライントレースを行う

考え方

ラインセンサーは2つ用いることにします。それぞれ、黒いラインを挟んで設置されていることとします。


上図で、下から動いてきたとします。直線部分では両センサーとも白を検出しているはずです。カーブは右側に曲がっているので、右のセンサーが黒を検出します。この場合、ロボットは右に曲がらなければいけないので、右の車輪のスピードを遅くします。遅くしすぎると曲がりすぎて、またコースを外れる可能性があるので、うまく設定します。
黒から脱出したころに遅くした右の車輪のスピードを元に戻します。
楕円コースの場合、この内側のセンサーのみでうまくすれば、ずっと回っていられます。しかしながら、曲率がいつも同じではないので、そう簡単にはいきません。
そのため、もう一つのセンサーが黒を検出したら左の車輪を遅くするようにプログラムします。

次のプログラムは、ロボットの個体差で正しく動かない場合があります(というか、動くはずがないと思うこと)。その場合は定数を適切に直してください。また、上記のアルゴリズムを少し現実に即したように書き換えています。
どこが少し違うか読んで理解してください。
意味も分からず、動いて万歳という人が最近目立つので、必ず理解してください。ここが基礎なので、基礎が出来ていないと、この先で訳が分からなくなります。

簡単なライントレース
#include "vs-wrc003.h"

void startInit(char* str);      // プロトタイプ宣言

void main(void)
{
    UINT left, right;
    const int true = 1;
    const int RightThreshold = 300;
    const int LeftThreshold  = 300;
    const int SPEED = 70;
    const int SLOW  = 30; 
    const int LEFT  = 1;
    const int RIGHT = 2;
    startInit("Line Trace test\n");
    while(true){
        left  = AdRead(LEFT);
        right = AdRead(RIGHT);
        while(right<RightThreshold) {    //左に寄って黒を検出したら
            LED(1);
            Mtr_Run(SLOW,SPEED,0,0);        // 右のモーターを遅くする
            Wait(50);
            left  = AdRead(LEFT);
            right = AdRead(RIGHT);
        }
        while(left<LeftThreshold) {     // 右に寄って黒を検出したら
            LED(2);
            Mtr_Run(SPEED,SLOW,0,0);       // 左のモーターを遅くする
            Wait(50);
            left  = AdRead(LEFT);
            right = AdRead(RIGHT);
        }
        Mtr_Run(SPEED,SPEED,0,0);          // スピードを元に戻す
        LED(0); 
        Wait(50);
    }
}

void startInit(char* str)
{
  const BYTE MainCycle = 60;
  Init((BYTE)MainCycle);        //CPUの初期設定
  InitSci3(CBR_9600,non,1);     // シリアル転送設定(printf)
  Mtr_Run(0, 0, 0, 0);          // これをやらないと色々と動かない。モーター停止
  LED(3);                       // 緑橙のLED点灯
  while(!getSW());              // スイッチが押されるまで待つ
  LED(0);                       // LED消灯
  printf(str);
}
 1 このロボット用ヘッダ
 2 
 3 
 4 
 5 
 6 
 7 unsighed intと同じ
 8 定数の設定
 9 
10 
11 
12 
13 
14 
15 
16 
17 
18 
19 
20 
21 
22 
23 
24 
25 
26 
27 
28 
29 
30 
31 
32 
33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 
47 
48