//
#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）のダウンロードはこちら