V850でライントレースーカー(ソフトウェア編)

ここでは、ライントレースカーを制御するソフトウェアについて説明します。V850によるライントレースカーもこれで最後です。ダイソーで売っている画用紙８枚をセロテープとホッチキスで引っ付けてコースを作りました。連続する直角カーブを正しく曲がるようにするのが結構難しかったです。100%正しく曲がれる訳ではないですが、とりあえず完成です。

ソフトの流れ

ソフトの大まかな流れは右図のようになります。まず初期設定では入出力ポートのレジスタ設定、モータ制御用PWMやタイマーのレジスタ設定などを行います。レジスタ設定は、マイコンごとに異なるのでいつもマニュアルとにらめっこになるのですが、V850にはAppliletと呼ばれる設定ソフトがあります。下図のように、レジスタの詳細を知らなくても、ポートの入出力やNチャンネルオープンドレイン、シリアル用UART、タイマー割り込みなどをGUIで設定できるため、とても便利です。設定項目はC言語で出力され、main関数が呼び出される前に、この辺りの設定がされるようになっています。モーター速度を制御するPWMもApplietで設定し、PWMの周期は2msec（周波数500Hz）にしました。「V850でライントレースカー（メイン回路編）」に書いたように、1msecだと高周波のノイズ音が聞こえたからです。また、10msecのタイマー割り込みを用いて、時刻の取得とLEDの点滅を行っています。

メインプログラムは、次のようになっています。フォトセンサー出力の取得は、センサーからの入力ポート（私の回路の場合、ポート33,34,41）から値(0は黒、1は白を示す)をREADするだけです。それを左、中央、右の順で、3bitのデータ（変数 pi）として格納しています。タイマー割り込みを使用してフォトセンサーの読み込み間隔を計測したところ、10msecの割り込みで約600回whileループを回っていたので、フォトセンサーのサンプリングは60KHz（秒６万回）ぐらいです。

void main( void )
{
    int pi;
        
    /* LED点滅スタート(500msec周期) */
    TMP0_Start();

    /* モータスタート */
    MotorStart();
        
    /* フォトインタラプタセンサースイッチ */
    P3L.5 = 1;

    while (1)
    {
        /* P3L.4 : PI Left Sensor Input */
        /* P3L.3 : PI Center Sensor Input */
        /* P4.1  : PI Right Sensor Input */
        
        /* P5.0  : PI Left  LED */
        /* P5.1  : PI Right LED */

        /* P3L.2 : Mortor Left  LED */
        /* P3L.6 : Mottor Right LED */

        /* フォトセンサーの出力 */
        /*     LEFT           CENTER         RIGHT   */
        pi = (P3L.4 << 2) | (P3L.3 << 1) | (P4.1 << 0);

        /* 現在時刻の取得 */
        g_curTime = g_TimeCounter;
        g_curTmReg = TP0CNT;
        
        /* ライントレース結果をLEDに反映 */
#if LED_SWITCH != 0
        PISensorLED(pi);
#endif
        /* モーター速度の決定 */
        SpeedControl(pi);

        /* モーター速度の更新 */
        MotorAccel();

        /* 状態の記録 */
        TraceMem(pi);

        /* 前回のPI */
        g_prePI = pi;

        g_loopCounter++;
    }
}

モーター速度の決定では、変数 pi とこれまでの状態（ライントレースの履歴）を元にモーター速度を決めています。基本は3個のセンサーで右側が黒(pi = 100や110）の場合は、右モーターを減速し、左モーターを加速します。反対に左側が黒(pi = 001や011）の場合には、左モーターを減速し、右モーターを加速します。問題は3個のセンサーがすべて白(pi = 111)になったときです。その場合には、pi = 111になる直前の状態が pi = 101の場合は、ラインを少し外れただけであり、左右のモーター速度を合わせるようにそれぞれ加速、減速しています。そうでなければ、過去200msec以内のセンサー値やモーター速度を元に判断するようにしています。200msecは経験的な値ですが、「V850でライントレースカー（車体編）」で調べたように、ライントレースカーの最高速度が12.4cm/secでしたので、おおよそ2cmぐらいの距離を元に判断していることになります。
モーター速度の更新は、上で求めたモーター速度、即ちPWMのデューティ比を設定しています。左右のモーターの正転方向の入力は、それぞれタイマーTMP2（ポート92）とタイマーTMP3（ポート96）に接続されており、TP2CCR1、TP3CCR1を変更することでデューティ比を設定できます。
状態の記録では、時刻、フォトセンサーの値 pi、左右のモーター速度を配列に覚えておくようにしています。フォトセンサーの値が変わった時だけ記録するようにしており、512個分の過去データを記録しています。
プログラムの説明はこれで終わりです。もっと詳しい内容はダウンロードしてみてください。

当初、直角カーブをうまく曲がることができなかったのですが、USBシリアルで状態（時刻、フォトセンサーの値、モーター速度など）を次のように出力するようにしました。

time                                   pi   leftM  rightM  interval
--------------------------------------------------------------------
55250msec[ 5525][  BC][  202][2833660]=101, 32053, 16053,  390msec
55640msec[ 5564][ DEA][  203][2855390]=001, 31974, 32000,    1msec
55642msec[ 5564][2869][  204][2855391]=101, 32027, 32053,    1msec
55643msec[ 5564][42B6][  205][2855392]=001, 31948, 32000,  196msec
55839msec[ 5583][BFDC][  206][2864809]=101, 16053, 32053,  269msec
56108msec[ 5610][AB09][  207][2879736]=100, 32000, 31974,   49msec 直角右
56156msec[ 5615][8796][  208][2882008]=110, 32000, 16000,   50msec
56206msec[ 5620][7F71][  209][2884355]=111, 32000, 16000, 1064msec 右旋回
57270msec[ 5727][ 119][  210][2932840]=110, 32000, 16000,   73msec
57343msec[ 5734][404E][  211][2936337]=111, 32000, 16000,   16msec
57359msec[ 5735][BF16][  212][2937028]=101, 32053, 16053,   78msec
57437msec[ 5743][94F3][  213][2941287]=111, 32000, 31934,   20msec 右行過ぎ
57457msec[ 5745][8E1D][  214][2942121]=011, 31960, 16040,  166msec
57622msec[ 5762][3189][  215][2950054]=010, 20000, 31974,    1msec
57624msec[ 5762][57B6][  216][2950088]=000, 20000, 31090,  111msec 直角右
57735msec[ 5773][6FEB][  217][2956107]=100, 20026, 19974,    2msec
57738msec[ 5773][A112][  218][2956164]=101, 21535, 18571,    9msec
57746msec[ 5774][851E][  219][2956556]=111, 32000, 30094, 1067msec 右旋回
58813msec[ 5881][3CBC][  220][3005166]=110, 32000, 16000,   83msec
58896msec[ 5889][85D5][  221][3009174]=111, 32000, 16000,   12msec
58909msec[ 5890][C02D][  222][3009705]=101, 32053, 16053,  115msec
59024msec[ 5902][5E29][  223][3016038]=111, 39974, 40000,   31msec 右行過ぎ
59056msec[ 5905][7549][  224][3017696]=011, 16000, 32000,   10msec
59065msec[ 5906][69D4][  225][3018087]=001, 16000, 32000,    1msec
59067msec[ 5906][8B75][  226][3018106]=000, 20000, 31974,  114msec 直角右
59180msec[ 5918][ 69C][  227][3024232]=001, 19974, 20026,    2msec
59182msec[ 5918][3643][  228][3024285]=101, 18675, 21431,   10msec
59191msec[ 5919][2572][  229][3024708]=111, 31190, 31934, 1205msec 右旋回
60397msec[ 6039][89DF][  230][3079655]=110, 32000, 16000,   87msec

このようなログを出力することで、直角カーブ時のセンサー値やモーター速度がどのようになっているかが分かり、ほぼ間違うことなく曲がれるようにしました。しかし、連続した直角カーブでは、直角カーブを曲がりきった直後に次の直角カーブがくるため、カーブ部分に斜めに入ることが多く、まだ間違うことがあります。ラインの太さも一定ではなく、実際のモーター速度も分からないため、このあたりが限界のような気もします。フォトセンサーではなく、カメラを使えば多くの画素で判断できるので、より正しく判定できると思います。

このページのトップへ