連載
» 2013年05月07日 10時00分 公開

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

[三月兎,MONOist]
前のページへ 1|2|3       

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

 まず、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)。


えみ

わーい。速くなったー!


北上

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


えみ

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


北上

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


えみ

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


北上

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


えみ

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


北上

なら、焼肉とかどう?


えみ

ワーイ!!





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

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

ロボット開発の最前線

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

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


前のページへ 1|2|3       

Copyright © ITmedia, Inc. All Rights Reserved.