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

/************************************************/
/*  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);
    }
}

/************************************************/
/*  モーター処理（前進）  （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);        //右左モーター停止
}

/************************************************/
/*  モーター処理（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;
                }
            }
        }
    }
}