走行中の姿勢制御を実装し、目標走行時間を突破せよ!!マイクロマウスで始める組み込み開発入門(15)(2/3 ページ)

» 2013年07月02日 10時00分 公開
[三月兎,MONOist]

 それでは、前回作成したプログラムに姿勢制御の処理を追加していきましょう。

 まず、「ctrl.h」に姿勢制御用の閾値とゲインを追加します(ソースコード1)。


//
#define CTRL_WALL_THRESHOLD_R 200         //右壁制御閾値
#define CTRL_WALL_THRESHOLD_L 200         //左壁制御閾値
//
#define CTRL_WALL_THRESHOLD_F 235         //区画中央閾値
//
#define CTRL_GAIN             0.05        //壁制御ゲイン値
ソースコード1 「ctrl.h」に姿勢制御用の閾値とゲインを追加する

 モーター処理(前進)の関数に、壁チェックを行うためのフラグ「G_WallCheckFlag」を立て、前壁を見て走行距離を調整するプログラムを追加します(ソースコード2)。

/************************************************/
/*    モーター処理(前進) (ctrl_mtr_forward) */
/************************************************/
/*    距離、目標速度を指定して左右のモーターを  */
/*    前進する                                  */
/*----------------------------------------------*/
/*    IN:int i_block … 区画数(半区画=1)    */
/*        int i_speed … 目標速度               */
/************************************************/
void ctrl_mtr_forward(int i_block, int i_speed)
{
 
    (……中略……)
 
    //壁チェックあり
    G_WallCheckFlag = FLAG_ON;
 
    (……中略……)
 
    //前壁で距離を調整
    if(ctrl_wall_chk_f(WALL_THRESHOLD_F) == WALL_ON)
    {
        //前壁あり
        if(ctrl_wall_chk_f(CTRL_WALL_THRESHOLD_F) == WALL_OFF)
        {
            //中央に達していない
            while(ctrl_wall_chk_f(CTRL_WALL_THRESHOLD_F) == WALL_OFF)
            {
            }
        }
    }
 
    (……中略……)
 
}
ソースコード2 「ctrl.c」の「ctrl_mtr_forward」関数に、前壁があったときに走行距離を調整する処理を追加する

 「G_WallCheckFlag」は、左右の壁をチェックするためのフラグです。前進中は左右の壁をセンシングしながら走行しますが、旋回するときには外します(ソースコード3)。

/************************************************/
/*    モーター処理(旋回)    (ctrl_mtr_turn) */
/************************************************/
/*    回転角度を指定して旋回する                */
/*----------------------------------------------*/
/*    IN:int i_angle … 旋回角度(°)         */
/*                      > 0 … 右旋回           */
/*                      < 0 … 左旋回           */
/************************************************/
void ctrl_mtr_turn(int i_angle)
{
 
    (……中略……)
 
    //壁チェックなし
    G_WallCheckFlag = FLAG_OFF;
 
    (……中略……)
 
}
ソースコード3 「ctrl.c」の「ctrl_mtr_turn」関数では、壁チェックのフラグを外す

 次に、走行中に姿勢制御をする関数「ctrl_wall_adjust」を作成します(ソースコード4)。壁までの距離をセンシングし、左右のモーター速度を変えながら走るので、マイクロマウスは細かく蛇行します。

/************************************************/
/*    右左壁で速度調整      (ctrl_wall_adjust)*/
/************************************************/
/*    右左壁からの位置でモーター速度を調整する  */
/*----------------------------------------------*/
/*    RET:int    … 調整値                      */
/************************************************/
int ctrl_wall_adjust(void)
{
    //右左壁で速度調整
    int ret;                        //戻り値(調整値)
    int rightwall;                  //右壁有無
    int leftwall;                   //左壁有無
    int rightdistance;              //右壁接近
    int leftdistance;               //左壁接近
    //壁チェックなしのときは、0を返す
    if(G_WallCheckFlag == FLAG_OFF)
    {
        return 0;
    }
    //調整値クリア
    ret = 0;
    //右左壁有無情報取得
    rightwall = ctrl_wall_chk_r(WALL_THRESHOLD_R);
    leftwall = ctrl_wall_chk_l(WALL_THRESHOLD_L);
    //右左壁接近情報取得
    rightdistance = ctrl_wall_chk_r(CTRL_WALL_THRESHOLD_R);
    leftdistance = ctrl_wall_chk_l(CTRL_WALL_THRESHOLD_L);
    //右壁チェック
    if(rightwall == WALL_ON && rightdistance == WALL_ON)
    {
        //右壁あり&右壁接近
        ret = (int)((float)G_SensorR * CTRL_GAIN); 
    }
    //左壁チェック
    if(leftwall == WALL_ON && leftdistance == WALL_ON)
    {
        //左壁あり&左壁接近
        ret = -(int)((float)G_SensorL * CTRL_GAIN); 
    }
    //
    return ret;
}
ソースコード4 「ctrl.c」に壁をセンシングして、左右のモーター速度を調整する「ctrl_wall_adjust」関数を作成する

 この「ctrl_wall_adjust」関数は、タイマー割り込み「int_cmt0」から呼び出されます(ソースコード5)。

/************************************************/
/*    CMT0割り込み処理            (int_cmt0)  */
/************************************************/
/*    タイマー割り込み(1ms)                   */
/************************************************/
void int_cmt0(void)
{
 
    (……中略……)
 
    //右左壁で速度調整
    adjust = ctrl_wall_adjust();
        //MTU設定
        ctrl_mtu(G_CurrentSpeedR + adjust, G_CurrentSpeedL - adjust);
 
    (……中略……)
 
}
ソースコード5 「intrpt.c」のint_cmt0にctrl_wall_adjust関数の呼び出しを追加する

 それでは、プログラムをビルドして、迷路内でマイクロマウスを走らせてみましょう!

>>ソースコード(Mouse201307.lzh)のダウンロードはこちら

動画1 姿勢制御をして右手法で迷路内を走行するPi:Co Classic。途中で壁にぶつかっても持ち直して走行を続けている。

最初の目標は「5分間の連続走行」

えみ

すごーい! ちゃんと迷路内を走ってますよ〜。


北上

おー!! いい感じだね。


えみ

姿勢制御ってもっと難しいのかと思っていたら、意外とカンタンなんですねぇ。


北上

今回は、優しい方法を実装したからね。それにカンタンに実装できたのは、階層構造化をしっかりやっているからだよ。


えみ

ここまできたら、後はゴールを目指すだけですね!


北上

う〜ん。それはロボットの開発方針次第だよ。もっと詰めておいた方がいいこともあるしね。


えみ

えぇ〜っ!! まだあるんですかぁ〜?


北上

実際、上を見たらキリがないっていうくらい、いろいろあるよ〜。


えみ

例えば、どんなことですか?



Copyright © ITmedia, Inc. All Rights Reserved.