台形駆動で脱調を克服！ ステッピングモーターをスピードアップ：マイクロマウスで始める組み込み開発入門（13）（3/3 ページ）

[三月兎，MONOist]

　それでは、前回作成したプログラムに台形駆動の処理を追加してみましょう。

　まず、intrpt.cのタイマー割り込み関数「int_cmt0」に、モーター制御を追加します（ソースコード1）。

/************************************************/
/*  CMT0割り込み処理            （int_cmt0）    */
/************************************************/
/*  タイマー割り込み（1ms）                     */
/************************************************/
void int_cmt0(void)
{
    //タイマー割り込み（1ms）
    CMT0_INT_F = 0;            //フラグクリア
    //タイマーカウント
    G_TimerCount++;            //1msごとにカウントアップ
    //モーター制御
    if(G_CurrentSpeedR != 0 || G_CurrentSpeedL != 0)
    {
        //加減速
        G_CurrentSpeedR += G_AccelValue;
        G_CurrentSpeedL += G_AccelValue;
        //最低速度チェック
        if(G_CurrentSpeedR != 0 && G_CurrentSpeedR < MIN_SPEED)
        {
            G_CurrentSpeedR = MIN_SPEED;
        }
        if(G_CurrentSpeedL != 0 && G_CurrentSpeedL < MIN_SPEED)
        {
            G_CurrentSpeedL = MIN_SPEED;
        }
        //最高速度チェック
        if(G_CurrentSpeedR > G_MaxSpeedR)
        {
            G_CurrentSpeedR = G_MaxSpeedR;
            if(G_AccelCount == 0)
            {
                G_AccelCount = G_StepCountR;
            }
        }
        if(G_CurrentSpeedL > G_MaxSpeedL)
        {
            G_CurrentSpeedL = G_MaxSpeedL;
            if(G_AccelCount == 0)
            {
                G_AccelCount = G_StepCountL;
            }
        }
        //MTU設定
        ctrl_mtu(G_CurrentSpeedR, G_CurrentSpeedL);
    }
}

ソースコード1　「intrpt.c」内のタイマー割り込み関数「int_cmt0」にモーター制御を追加

　続いて、モーター処理（前進）とモーター処理（旋回）に、現在速度を最低速度に設定する記述と、最高速度、加速値の処理を追加します（ソースコード2）。

/************************************************/
/*  モーター処理（前進）  （ctrl_mtr_forward）  */
/************************************************/
/*  距離、目標速度を指定して左右のモーターを    */
/*  前進する                                    */
/*----------------------------------------------*/
/*  IN :int i_block … 区画数（半区画＝1）      */
/*      int    i_speed    … 目標速度           */
/************************************************/
void ctrl_mtr_forward(int i_block, int i_speed)
{
    //モーター処理（前進）
    int stepCount;                        //走行ステップ数
    int adjust;                           //モーター速度調整値
    //ステップカウントクリア
    G_StepCountR = 0;
    G_StepCountL = 0;
    //現在速度を最低速度に設定する
    G_CurrentSpeedR = MIN_SPEED;
    G_CurrentSpeedL = MIN_SPEED;
    //走行ステップ数算出
    stepCount = (int)(((float)i_block / (float)ONE_BLOCK)
                    * ((float)BLOCK_LENGTH / (float)STEP_LENGTH));
    //最高速度
    G_MaxSpeedR = i_speed;
    G_MaxSpeedL = i_speed;
    //加速値
    G_AccelValue = ACCEL_VALUE;
    G_AccelCount = 0;
    //モーター処理（前進）
    drv_mtr(MTR_RUN, MTR_RUN);          //右左モーター前進
    //目標地点まで走行
    ctrl_mtr_wait(stepCount);
    //モーター処理（停止）
    drv_mtr(MTR_STOP, MTR_STOP);        //右左モーター停止
}

ソースコード2　モーター処理（旋回）も同様に記述を追加する

　モーター処理（wait）では、台形駆動の減速開始地点の判断を追加します。また、移動距離が短くて、最高速度に達する前に減速を開始する場合も考慮しておきます（ソースコード3）。

/************************************************/
/*  モーター処理（wait）    （ctrl_mtr_wait）   */
/************************************************/
/*  モーターが指定ステップ数まで回転するのを待つ*/
/*  減速地点になったら減速する                  */
/************************************************/
void ctrl_mtr_wait(int i_stepCount)
{
    while(G_StepCountR < i_stepCount || G_StepCountL < i_stepCount)    //左右のモーターが走行ステップ数に達するまでループ
    {
        if(G_AccelValue > 0)
        {
            //加速中
            if(G_AccelCount != 0)
            {
                if((i_stepCount - G_StepCountR < G_AccelCount) ||
                   (i_stepCount - G_StepCountL < G_AccelCount))
                {
                    //減速地点になったら減速を開始
                    G_AccelValue = -ACCEL_VALUE;
                }
            }
            else
            {
                if((i_stepCount <= G_StepCountR * 2) ||
                   (i_stepCount <= G_StepCountL * 2))
                {
                    //中間地点になったら減速を開始
                    G_AccelValue = -ACCEL_VALUE;
                }
            }
        }
    }
}

ソースコード3　台形駆動の減速開始地点を判断

＞＞ソースコード（Mouse201305.lzh）のダウンロードはこちら

　それでは、プログラムをビルドして、Pi:Co Classicの走りがどのように変わったかを確認してみましょう（動画2）。

動画2　台形駆動を追加しスピードアップした

わーい。速くなったー！

前回と比べると、加速しているのがよく分かるよね。

ですねぇ〜。苦労してプログラムを書いたかいがあります。

今回のプログラムは、迷路探索を終えて、最短走行をしたときに威力を発揮するはずだよ。

……てことは、次は迷路探索ですか!?

いやいや。その前に、センサーで壁の有無を判断して、迷路内を自走できるようにしないと！　ここからがいよいよマイクロマウス競技の本番だよ。

はい！ じゃあ、気合を入れるためにも、今日は何かおいしいもの食べに行きましょうよ。

なら、焼肉とかどう？

ワーイ!!

---

　脱調はステッピングモーターを扱う上で、必ず考慮すべき問題点です。台形駆動の方式は、他にもいろいろあります。ステッピングモーターの加速パターンは、本稿で紹介した等加速度方式以外にもSin関数近似曲線、指数関数近似曲線などがあります。モーターの特性や車体重量などの条件を考慮し、最適な加速方式を選択してください。

　次回は、センサーで壁の有無を判断して走るプログラムを作成します。お楽しみに！ （次回に続く）

ロボット開発の最前線

あらゆる技術の集大成といわれる「ロボット」をテーマに、産業、レスキュー、生活支援／介護分野などにおけるロボット開発の実情や、関連する要素技術、研究開発、ロボットコンテスト、ロボットビジネスの最新動向などをお届けする。

＞＞ロボット特集のTOPページはこちら