キット製作研修マイクロマウス研修（kora編）
HM-StarterKit

マイクロマウス研修（kora編）[16] 迷路探索の実装

キット製作研修
2019.02.19

こんにちは。koraです。

今回は、前回の記事で解説した迷路探索アルゴリズムを、HM-StarterKitに実装して迷路を走らせてみます。

迷路探索関連の関数の追加

search.hとsearch.c

サンプルプログラムのStep7から、search.hとsearch.cをコピーして、CS+のプロジェクトに加えます。これで、以下の関数が追加されます。

  • search_lefthand(x, y)
    左手方で探索する関数。今回は使用しません。
  • init_map(x, y)
    歩数マップの初期化関数。歩数マップ更新のたびに呼び出されます。
  • make_map(x, y, mask)
    ゴール座標(x, y)の歩数マップを更新する関数。
  • set_wall(x, y)
    光センサの情報をもとに、現在の座標(x, y)の壁情報を設定する関数。
  • is_unknown(x, y)
    指定した座標(x, y)が未探索区間か否かを判定する関数。get_priority関数から呼ばれます。
  • get_priority(x, y, dir)
    現在の座標(x, y)から方角dirに移動する優先度を取得する関数。指定した方角が未探索の場合や直進の場合、高い優先度になります。get_nextdir関数から呼ばれます。
  • get_nextdir(x, y, mask, *dir)
    次に行くべき方向を取得する関数。
  • search_adachi(gx, gy)
    指定したゴール座標(gx, gy)に向かって、足立法で探索する関数。

構造体、グローバル変数、定数の追加

mytypedef.h

mytypedef.hに次の列挙型(enum)と構造体の型(struct)の宣言を追加します。

typedef enum
{
    front=0,    //前
    right=1,    //右
    rear=2,     //後
    left=3,     //左
    unknown,    //方向不明
}t_local_dir;   //自分から見た方向を示す列挙型

typedef enum
{
    north=0,
    east=1,
    south=2,
    west=3,
}t_direction;

typedef struct
{
    short x;
    short y;
    t_direction dir;
}t_position;

typedef struct
{
    unsigned char north:2;  //北の壁情報
    unsigned char east:2;   //東の壁情報
    unsigned char south:2;  //南の壁情報
    unsigned char west:2;   //西の壁情報
}t_wall;                    //壁情報を格納する構造体(ビットフィールド)
glob_var.h

構造体をグローバル変数として使えるよう、glob_var.hに以下を追加します。

GLOBAL t_control      con_fwall;                     //制御構造体
GLOBAL t_position     mypos;                         //自己座標
GLOBAL t_wall         wall[MAZESIZE_X][MAZESIZE_Y];  //壁の情報を格納する構造体配列
GLOBAL unsigned char  map[MAZESIZE_X][MAZESIZE_Y];   //歩数マップ
static_parameters.h

探索に必要な定数を追加します。

#define MAZESIZE_X  (32) //迷路の大きさ(MAZESIZE_X * MAZESIZE_Y)迷路
#define MAZESIZE_Y  (32) //迷路の大きさ(MAZESIZE_X * MAZESIZE_Y)迷路

#define UNKNOWN 2        //壁があるかないか判らない状態の場合の値
#define NOWALL  0        //壁がないばあいの値
#define WALL    1        //壁がある場合の値
#define VWALL   3        //仮想壁の値(未使用)
parameters.h

探索を開始させるときは、マウスの前に手をかざして、光センサ変化を読み取らせます。そのための光センサの閾値をparameters.hに追加します。

#define	SEN_DECISION	2000	//メニュー決定用の光センサ閾値
init.c

init_maze関数をinit.cに追加します。

/*****************************************************************************************
迷路情報の初期化

*****************************************************************************************/
void init_maze(void)	//迷路情報の初期化
{
    int i,j;
	
    for(i = 0; i < MAZESIZE_X; i++)
    {
        for(j = 0; j < MAZESIZE_Y; j++)
        {
            wall[i][j].north = wall[i][j].east = wall[i][j].south = wall[i][j].west = UNKNOWN;	//迷路の全体がわからない事を設定する
        }
    }
	
    for(i = 0; i < MAZESIZE_X; i++)
    {
        wall[i][0].south = WALL;		//四方の壁を追加する(南)
        wall[i][MAZESIZE_Y-1].north = WALL;	//四方の壁を追加する(北)
    }

    for(j = 0; j < MAZESIZE_Y; j++)
    {
        wall[0][j].west = WALL;			//四方の壁を追加する(西)
        wall[MAZESIZE_X-1][j].east = WALL;	//四方の壁を追加する(東)
    }

    wall[0][0].east = wall[1][0].west = WALL;	//スタート地点の右の壁を追加する	
}

init_all関数内にinit_maze関数を呼び出す行を追加します。

    init_maze();

探索のテスト

my_hm_starterkit.c

サンプルプログラムを参考にmain関数を次のように書き換えます。これで4×4マスの迷路を探索できるはずです。

void main(void)
{
    init_all();

    // ブザー
    BEEP();

    while(1)
    {
        //センサーの前に手をかざしてスタート
        if(sen_fr.value + sen_fl.value + sen_r.value + sen_l.value > SEN_DECISION * 4)
        {
            // ブザー
            BEEP();

            // 積分用の変数を初期化
            I_tar_ang_vel = 0;
            I_ang_vel = 0;
            I_tar_speed = 0;
            I_speed = 0;

            // 車体方向を初期化
            degree = 0;

            // ジャイロの参照値を取得
            gyro_get_ref();

            // ブザー
            BEEP();
			
            // 車体の座標と方角を初期化
            mypos.x = mypos.y = 0;
            mypos.dir = north;

            // 足立法でゴール座標(3, 3)まで探索する
            search_adachi(3, 3);

            // ゴールしたら180度回転して方角を更新する
            turn(180,TURN_ACCEL,TURN_SPEED,RIGHT);
            mypos.dir = (mypos.dir+6) % 4;

            // ゴールしたことをアピール
            wait_ms(100);
            BEEP();
            wait_ms(100);
            BEEP();

            // モータの電源OFF
            MOT_POWER_OFF;

            // 終了したことをアピール
            BEEP();
        }
    }
}

このプログラムで実際に探索を行った動画がこちらです。
ふらつきながらもなんとかゴールすることができました。

パラメータの調整

マウスが迷路を走るとき、光センサやタイヤなどの個体差に加え、環境光、路面状況、壁の反射率の違いなどで走行が安定しません。安定させるためにはparameter.hに記載されたパラメータを微調整する必要があります。

  • タイヤの直径
  • 各光センサの閾値
  • LED発光からAD変換開始までの時間
  • PIDゲイン

    • などを調整した結果がこの動画です。
    比較的まっすぐに走るようになりました。

    次回

    4×4の小さな迷路ですが、ゴールまでの探索に成功しました。
    しかし、実際のマイクロマウス競技では16×16や32×32になりますので、もっと高速化が必要です。
    次回は高速化のため「スラローム探索（通称スラ探）」を追加しようと思います。

タイトルとURLをコピーしました