走行中の姿勢制御を実装し、目標走行時間を突破せよ!!：マイクロマウスで始める組み込み開発入門（15）（2/3 ページ）
市販の組み立てキットで「マイクロマウス」の開発を進める北上くんとえみちゃん。これまで開発してきたプログラムをベースに、センサーで壁の有無を判断しながら迷路内を自律走行させることに成功しました。最終回となる今回は、さらなる安定走行を目指し、「姿勢制御」を実装します。
それでは、前回作成したプログラムに姿勢制御の処理を追加していきましょう。
まず、「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 //壁制御ゲイン値
モーター処理（前進）の関数に、壁チェックを行うためのフラグ「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)
{
}
}
}
（……中略……）
}
「G_WallCheckFlag」は、左右の壁をチェックするためのフラグです。前進中は左右の壁をセンシングしながら走行しますが、旋回するときには外します（ソースコード3）。
/************************************************/
/* モーター処理（旋回） （ctrl_mtr_turn） */
/************************************************/
/* 回転角度を指定して旋回する */
/*----------------------------------------------*/
/* IN：int i_angle … 旋回角度（°） */
/* > 0 … 右旋回 */
/* < 0 … 左旋回 */
/************************************************/
void ctrl_mtr_turn(int i_angle)
{
（……中略……）
//壁チェックなし
G_WallCheckFlag = FLAG_OFF;
（……中略……）
}
次に、走行中に姿勢制御をする関数「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;
}
この「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);
（……中略……）
}
それでは、プログラムをビルドして、迷路内でマイクロマウスを走らせてみましょう！
＞＞ソースコード（Mouse201307.lzh）のダウンロードはこちら
最初の目標は「5分間の連続走行」
すごーい！ ちゃんと迷路内を走ってますよ〜。
おー!! いい感じだね。
姿勢制御ってもっと難しいのかと思っていたら、意外とカンタンなんですねぇ。
今回は、優しい方法を実装したからね。それにカンタンに実装できたのは、階層構造化をしっかりやっているからだよ。
ここまできたら、後はゴールを目指すだけですね！
う〜ん。それはロボットの開発方針次第だよ。もっと詰めておいた方がいいこともあるしね。
えぇ〜っ!! まだあるんですかぁ〜？
実際、上を見たらキリがないっていうくらい、いろいろあるよ〜。
例えば、どんなことですか？
Copyright © ITmedia, Inc. All Rights Reserved.