台形駆動で脱調を克服！ ステッピングモーターをスピードアップ：マイクロマウスで始める組み込み開発入門（13）（3/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);
}
}
続いて、モーター処理（前進）とモーター処理（旋回）に、現在速度を最低速度に設定する記述と、最高速度、加速値の処理を追加します（ソースコード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); //右左モーター停止
}
モーター処理（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;
}
}
}
}
}
＞＞ソースコード（Mouse201305.lzh）のダウンロードはこちら
それでは、プログラムをビルドして、Pi:Co Classicの走りがどのように変わったかを確認してみましょう（動画2）。
わーい。速くなったー！
前回と比べると、加速しているのがよく分かるよね。
ですねぇ〜。苦労してプログラムを書いたかいがあります。
今回のプログラムは、迷路探索を終えて、最短走行をしたときに威力を発揮するはずだよ。
……てことは、次は迷路探索ですか!?
いやいや。その前に、センサーで壁の有無を判断して、迷路内を自走できるようにしないと！ ここからがいよいよマイクロマウス競技の本番だよ。
はい！ じゃあ、気合を入れるためにも、今日は何かおいしいもの食べに行きましょうよ。
なら、焼肉とかどう？
ワーイ!!
脱調はステッピングモーターを扱う上で、必ず考慮すべき問題点です。台形駆動の方式は、他にもいろいろあります。ステッピングモーターの加速パターンは、本稿で紹介した等加速度方式以外にもSin関数近似曲線、指数関数近似曲線などがあります。モーターの特性や車体重量などの条件を考慮し、最適な加速方式を選択してください。
次回は、センサーで壁の有無を判断して走るプログラムを作成します。お楽しみに！ （次回に続く）
ロボット開発の最前線
Copyright © ITmedia, Inc. All Rights Reserved.