行け!! マイクロマウス！ ステッピングモーターで前進だ：マイクロマウスで始める組み込み開発入門（12）（3/3 ページ）

[三月兎，MONOist]

　ここまでの内容で、モーターを動かす準備が整いました。

　「drv.h」にモーター制御用の変数を設定し、「drv.c」内に概要図で示した3つの関数を作成します（ソースコード7）。

/************************************************/
/*    モーター励磁            （drv_mtr_pw）    */
/************************************************/
/*    モーターの励磁のON/OFFを行う              */
/*----------------------------------------------*/
/*    IN：int i_stat … MTR_ON（0） ：励磁      */
/*                      MTR_OFF（1）：励磁解除  */
/************************************************/
void drv_mtr_pw(int i_stat)
{
    //モーター励磁 ON/OFF
    MTR_POWER = i_stat;
}
 
/************************************************/
/*   モータードライバ          （drv_mtr）      */
/************************************************/
/*   回転方向を指定して左右モーターMTUのカウント*/
/*   をスタートする                             */
/*----------------------------------------------*/
/*   IN：int i_direction_r … 右モーター回転方向*/
/*       int i_direction_l … 左モーター回転方向*/
/*                         MTR_STOP（0） ：停止 */
/*                         MTR_RUN （1） ：前進 */
/*                         MTR_BACK（-1）：後進 */
/************************************************/
void drv_mtr(int i_direction_r, int i_direction_l)
{
    //右モーター
    switch(i_direction_r)                //方向
    {
        case MTR_STOP:                   //停止
            MTU_MTR_R = MTU_OFF;         //右モーターMTU停止
            break;
        case MTR_RUN:                    //前進
            MTR_CWCCW_R = MTR_FORWARD;   //CWCCW 前進 （*1）
            MTU_MTR_R   = MTU_ON;        //右モーターMTUカウント開始
            break;
        case MTR_BACK:                   //後進
            MTR_CWCCW_R = MTR_BACKWARD;  //CWCCW 後進 （*2）
            MTU_MTR_R   = MTU_ON;        //右モーターMTUカウント開始
            break;
    }
    //左モーター
    switch(i_direction_l)                //方向
    {
        case MTR_STOP:                   //停止
            MTU_MTR_L = MTU_OFF;         //左モーターMTU停止
            break;
        case MTR_RUN:                    //前進
            MTR_CWCCW_L = MTR_FORWARD;   //CWCCW 前進 （*1'）
            MTU_MTR_L   = MTU_ON;        //左モーターMTUカウント開始
            break;
        case MTR_BACK:                   //後進
            MTR_CWCCW_L = MTR_BACKWARD;  //CWCCW 後進 （*2'）
            MTU_MTR_L   = MTU_ON;        //左モーターMTUカウント開始
            break;
    }
}
 
/************************************************/
/*    モーターMTUドライバ      （drv_mtu）      */
/************************************************/
/*    モーターMTUの周期を設定する               */
/*----------------------------------------------*/
/*    IN：int i_cycle_r … 右モーター周期       */
/*        int i_cycle_l … 左モーター周期       */
/************************************************/
void drv_mtu(int i_cycle_r, int i_cycle_l)
{
    //モーターMTUの周期を設定する
    MTU_MTR_CYCLE_R = (unsigned short)i_cycle_r;
    MTU_MTR_CYCLE_L = (unsigned short)i_cycle_l;
}

ソースコード7　「drv.c」内に、3つのモータードライバ関数を追加する

　モータードライバ関数「drv_mtr」において、左右のモーターは、MTR_CWCCWに定数MTR_FORWARD（＝1）を出力した際に前進（ソースコード7　*1、*1'）、定数MTR_BACKWARD（＝0）を出力した際に後進（ソースコード7　*2、*2'）します。これは、Pi:Co Classicを組み立てるときに、ステッピングモーターのケーブルを片方クロスケーブルにし、物理的に電流の流れる向きを反転させているためです（画像5）。組み込み開発におけるプログラミングは、ハードウェアがどのような仕様になっているかを理解した上で行う必要があります。

画像5　右モーターをクロスケーブルにすることで、左右のモーターが同じ定数で動くようになり、プログラムが読みやすくなっている

　そして、「ctrl.h」と「ctrl.c」に関数を作成して、モータードライバ関数を呼び出します（ソースコード8）。「ctrl.h」には前述のソースコード2の物理的パラメータを記述します。

/************************************************/
/*    モーター励磁           （ctrl_mtr_pw）    */
/************************************************/
/*    モーターの励磁のON/OFFを行う              */
/*----------------------------------------------*/
/*    IN：int i_stat … MTR_ON （0）：励磁      */
/*                      MTR_OFF（1）：励磁解除  */
/************************************************/
void ctrl_mtr_pw(int i_stat)
{
    //モーター励磁 ON/OFF
    drv_mtr_pw(i_stat);
}
 
/************************************************/
/*    モーター処理（前進） （ctrl_mtr_forward） */
/************************************************/
/*    距離を指定して左右のモーターを前進する    */
/*----------------------------------------------*/
/*    IN：int i_block … 区画数（半区画＝1）    */
/************************************************/
void ctrl_mtr_forward(int i_block)
{
    //モーター処理（前進）
    int stepCount;                  //走行ステップ数
    int adjust;                     //モーター速度調整値
    //ステップカウントクリア
    G_StepCountR = 0;
    G_StepCountL = 0;
    //走行ステップ数算出
    stepCount = (int)(((float)i_block / (float)ONE_BLOCK)
                    * ((float)BLOCK_LENGTH / (float)STEP_LENGTH));
    //モーター処理（前進）
    drv_mtr(MTR_RUN, MTR_RUN);      //左右モーター前進
    //目標地点まで走行
    ctrl_mtr_wait(stepCount);
    //モーター処理（停止）
    drv_mtr(MTR_STOP, MTR_STOP);    //左右モーター停止
}
 
/************************************************/
/*    モーター処理（旋回）   （ctrl_mtr_turn）  */
/************************************************/
/*    回転角度を指定して旋回する                */
/*----------------------------------------------*/
/*    IN：int i_angle … 旋回角度（度）         */
/*                      > 0 … 右旋回           */
/*                      < 0 … 左旋回           */
/************************************************/
void ctrl_mtr_turn(int i_angle)
{
    //モーター処理（旋回）
    int angle;                //旋回角度
    int mtrDirectionR;        //右モーター回転方向
    int mtrDirectionL;        //左モーター回転方向
    int stepCount;            //走行ステップ数
    //回転方向を決定
    if(i_angle > 0)
    {
        //右旋回
        angle      = i_angle;
        mtrDirectionR = MTR_BACK;        //右モーター後進
        mtrDirectionL  = MTR_RUN;        //左モーター前進
    }
    else
    {
        //左旋回
        angle      = -i_angle;
        mtrDirectionR = MTR_RUN;         //右モーター前進
        mtrDirectionL  = MTR_BACK;       //左モーター後進
    }
    //ステップカウントクリア
    G_StepCountR = 0;
    G_StepCountL = 0;
    //走行ステップ数算出
    stepCount = (int)((float)TREAD_CIRCUIT
                    * ((float)angle / (float)360.0)
                    / (float)STEP_LENGTH);
    //モーター処理（旋回）
    drv_mtr(mtrDirectionR, mtrDirectionL);
    //目標地点まで走行
    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)    //左右のモーターが走行ステップ数に達するまでループ
    {
    }
}
 
/************************************************/
/*    モーターMTU制御            （ctrl_mtu）   */
/************************************************/
/*    モーターMTUの周期を設定する               */
/*----------------------------------------------*/
/*    IN：int i_speed_r … 右モータースピード   */
/*        int i_speed_l … 左モータースピード   */
/************************************************/
void ctrl_mtu(int i_speed_r,int i_speed_l)
{
    //右モーターMTUの周期とパルス幅を設定する
    float cycle_r;
    float cycle_l;
    //
    cycle_r = (float)(STEP_LENGTH / i_speed_r * MTU_MTR_CLOCK);
    cycle_l = (float)(STEP_LENGTH / i_speed_l * MTU_MTR_CLOCK);
    //
    drv_mtu((int)cycle_r, (int)cycle_l);
}

ソースコード8　「ctrl.c」に5つの関数を作成する

アプリケーションの作成

　ここまでくれば、後はアプリケーションでコントローラを呼び出すだけです。モード3に1区画（180mm）前進、モード4に右旋回、モード5に左旋回のプログラムを作成していきましょう（ソースコード9）。

/************************************************/
/*    モード3の処理              （app_mode3）  */
/************************************************/
/*    1区画走行                                 */
/************************************************/
void app_mode3(void)
{
    //MTUの周期を初期化する
    ctrl_mtu(MIN_SPEED ,MIN_SPEED);
    //モーター励磁ON
    ctrl_mtr_pw(MTR_ON);
    //0.5秒wait
    ctrl_wait_ms(500);
    //1区画前進して停止
    ctrl_mtr_forward(ONE_BLOCK);
    //モーター励磁OFF
    ctrl_mtr_pw(MTR_OFF);
}
 
/************************************************/
/*    モード4の処理              （app_mode4）  */
/************************************************/
/*    右旋回                                    */
/************************************************/
void app_mode4(void)
{
    //MTUの周期を初期化する
    ctrl_mtu(MIN_SPEED ,MIN_SPEED);
    //モーター励磁ON
    ctrl_mtr_pw(MTR_ON);
    //0.5秒wait
    ctrl_wait_ms(500);
    //右90度旋回して停止
    ctrl_mtr_turn(90);
    //モーター励磁OFF
    ctrl_mtr_pw(MTR_OFF);
}
　

ソースコード9　モード5に、右旋回と同様に左旋回プログラムを作成する

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

　では、プログラムをビルドして、Pi:Co Classicの動作を確認してみましょう（動画1）。

動画1　マイクロマウスが1区画前進、モードを切り替えて右旋回と左旋回をする

わーい！ 走った〜。ちゃんと旋回もしてますよ！

うん。カンペキだね！

でも、随分とゆっくりですねぇ〜。もう少し速く走れるとカッコいいのに……。

パルスの周期を早くすれば、スピードがアップするよ。

じゃあ、もっと速く走らせましょうよ！

もちろん、そうするつもりだよ!! ただ、ステッピングモーターは急加速すると、「脱調」して動かなくなるんだ。それを防ぐために、MTUの初期化でTGRCをTGRAのバッファレジスタとして使用してだね……（ブツブツ）。

（ヤバッ!! センパイのモーターが急加速しちゃった）

あ、センパイ！ ワタシ今日はおやつを持ってきたんですよ。

じゃ〜ん!! 桜餅と柏餅っ！ お茶した後で、じっくりとステッピングモーターをスピードアップさせる方法を聞かせてくださいよ〜。

おっ、いいねぇ〜。

じゃあ、お茶いれてくるよ。

　連載12回目にして、ようやくマイクロマウスがオリジナルプログラムで走りました！ ちょっとスピードが遅いですが、自分で書いたプログラムでロボットが動くと感動しますよね。

　今回のソースコードはかなり長くなりました。しかし、階層構造にしてあるおかげで、アプリケーション層のプログラムは思いの外シンプルです。これから先、マイクロマウスが実際に迷路を走行するようになると、階層構造の恩恵をますます感じられるはずです!!

　さて次回は、ステッピングモーターを動かすときに必ず意識しなければならない「脱調」を克服し、マイクロマウスの走行スピードをアップさせます。お楽しみに！ （次回に続く）

ロボット／ロボット開発 コーナー

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

＞＞コーナーTOPはこちらから