こんにちは。haraです。
前回は、迷路走破のためにサンプルプログラムのStep8の全体を見てみました。
今回は、Step8のプログラムの修正とパラメータの調整を行っていきたいと思います。
迷路走行のためのプログラム
迷路走破のためには、壁にぶつからずに安定した走行と前後左右の壁の有無を正確に把握することが必要になります。
壁にぶつからないようにする制御と壁の検出に関する基本的なアルゴリズムは、interrupt.cに実装されていました。
このファイルの中で定義されている割り込み関数は、int_cmt0、int_cmt1、int_cmt2、int_mot_r、int_mot_lの5つありましたが、走行に係る関数は、int_cmt0、int_cmt1、int_mot_r、int_mot_lです。
int_mot_rとint_mot_lは、モータへ与えたパルスのカウントであり、
int_cmt0は、1msecごとの割り込みで呼び出され、加速と必要ならば壁沿いの制御を行っており、init_cmt1は、0.25msecごとの割り込みで呼び出され、４つ可視光センサの読み取りを順に行っています。
つまり、 壁の検出と速度の指定が1msecごとに実行されていることがわかりました。
迷路走行のアルゴリズムは、search.cに実装されており、左手法と足立法の両方が実装されてることがわかります。
フルの迷路を走破するためには足立法を使いますので、search_adachi関数の中を見ていきます。
search_adachi関数では、
- 最初に半マス前進(全方向について実装されていましたが、結局最初は前進のみでした)
- 周囲の壁の有無によって次に進むべき方向を決定
- それに従って移動または回転
- 迷路の地図を更新
をゴールまで続けるようになっています。
また、search_adachi関数内で使われている移動のための関数は、run.cにあるstraight, straight_for_search, rotateの3つの関数でした。
これらの関数は、移動速度を設定後に設定した距離（目標パルス）に到達するまで待つようになっています。
以上のことから、調整するパラメータは、int_cmt0とint_cmt1で使われている壁の検出と壁沿い制御のパラメータだとわかりました。
プログラムの修正
迷路走破のためのサンプルプログラムの理解がほぼ終わったのですが、各関数はもう少し整理できそうでしたので修正をしていきます。
この修正は、研修の後半でスラローム走行の追加もあるので、その時に余計なバグを混入させないためです。
では、search_adachi関数を下のように修正していきます。
/*
引数gx,gyに向かって足立法で迷路を探索
*/
void search_adachi(int gx, int gy)
{
t_direction glob_nextdir; //次に向かう方向を記録する変数
int direction; // 進む方向
accel = SEARCH_ACCEL; // 迷路探索の移動加速度の設定
// 最初に進む方向を決定
direction = get_nextdir(gx, gy, MASK_SEARCH ,&glob_nextdir);
rotate(direction,1); // 進む方向（北か東)を向く
straight(HALF_SECTION, 0,SEARCH_SPEED,SEARCH_SPEED); // 半区画前進
update_coordinate_position(glob_nextdir); // 現在位置と向き(mypos)の更新と
// カウンタのリセット
set_wall(mypos.x, mypos.y); // 仮想壁をセット
while((mypos.x != gx) || (mypos.y != gy)) // ゴールするまで処理を繰返す
{
// 次に進む方向を決定
direction = get_nextdir(gx, gy, MASK_SEARCH ,&glob_nextdir);
move_adachi_one_section(direction); // 次の探索方向に1区画移動
update_coordinate_position(glob_nextdir); // 現在位置(mypos)の更新と
// カウンタのリセット
set_wall(mypos.x, mypos.y); // 仮想壁をセット
}
// ゴールしたので、半区画前進
straight_for_search(HALF_SECTION,SEARCH_SPEED,0);
}
この関数を見ると新たに実装する関数は、move_adachi_one_sectionとupdate_coordinate_positionです。
move_adachi_one_sctionは、get_nextdir関数で決定した探索方向への移動ですので、下のように書くことができます。
void move_adachi_one_section(int direction)
{
switch(direction)
{
case front: // 前進なので1区画進む
straight_for_search(SECTION, SEARCH_SPEED, SEARCH_SPEED);
break;
case right: // 右または左に超信地旋回する
case left:
straight_for_search(HALF_SECTION, SEARCH_SPEED, 0); // 半区画前進
rotate(direction,1); // 超信地旋回
straight(HALF_SECTION,0, SEARCH_SPEED, SEARCH_SPEED); // 半区画前進
break;
case rear: // 超信地旋回でUターン
straight_for_search(HALF_SECTION, SEARCH_SPEED, 0); // 半区画前進
rotate(right,2); // 回れ右
straight(HALF_SECTION, 0, SEARCH_SPEED, SEARCH_SPEED); // 半区画前進
break;
}
}
上の修正を見ると rearもright, leftとほぼ同じように見えますが、、超信地旋回からスラロームに変更する時に再度変更が必要と予想されますので、この関数を修正のままにしておきます。
また、rearを行うのは袋小路に入った場合ですので、移動誤差調整の行動も追加できそうです。
次に、update_coordinate_position関数は、移動した1区画分の座標を更新して、次の移動のためにモータのカウンタをリセットしますので、下のように書くことができます。
void update_coordinate_position(int dir)
{
mypos.dir = dir; // 自分の向いた方向を更新
switch(mypos.dir)
{
case north:
mypos.y++; // 北を向いた時はY座標を増やす
break;
case east:
mypos.x++; // 東を向いた時はX座標を増やす
break;
case south:
mypos.y--; // 南を向いた時はY座標を減らす
break;
case west:
mypos.x--; // 西を向いたときはX座標を減らす
break;
}
step_r = step_l = 0; // 進んだ距離カウント用変数をリセット
MTU.TSTR.BIT.CST3 = MTU.TSTR.BIT.CST4 = 1; // モータのカウントスタート
}
以上で、とりあえずのプログラムの修正は終了です。
動作パラメータの調整
これまでのプログラム修正を見ていると特にパラメータ調整に該当する場所がないのに気づきます。
結局のところ straight関数とrotate関数が正確に動けばいいのだとわかりました。
straight関数は、
void straight(int len, float init_speed, float max_sp, float tar_speed)
{
int obj_step; //目標ステップ数
max_speed = max_sp;
r_accel = accel; //加速度
if(init_speed < MIN_SPEED){
speed = MIN_SPEED;
}else{
speed=init_speed;
}
//目標速度が最低速度を下回らないようにする
if(tar_speed < MIN_SPEED)
{
tar_speed = MIN_SPEED;
}
if(init_speed < tar_speed){
min_speed = MIN_SPEED;
} else{
min_speed = tar_speed;
}
step_r = step_l = 0; // ステップ数カウントのリセット
con_wall.enable = true; // 壁制御を有効にする
obj_step = LEN2STEP(len); // 目標ステップ数を算出
MOT_CWCCW_R = MOT_CWCCW_L = MOT_FORWARD; // 前方に進む
MTU.TSTR.BIT.CST3 = MTU.TSTR.BIT.CST4 = 1; // カウントスタート
// 減速を開始し始めるところまで待つ
while( (len - STEP2LEN(step_r + step_l) ) >
( ( (tar_speed*tar_speed) - (speed*speed) ) / (-2*1000*accel) ));
r_accel = -accel; //減速する
while((step_r + step_l) < obj_step) ; //目標地点まで走行
MTU.TSTR.BIT.CST3 = 0; //モータのカウントをストップ
MTU.TSTR.BIT.CST4 = 0; //モータのカウントをストップ
}
と実装されており、rotate関数は、
void rotate(t_local_dir dir, int times)
{
int obj_step; //目標ステップ
int tar_speed = MIN_SPEED; //目標速度(固定)
con_wall.enable = false; //超信地旋回中は制御を切る
switch(dir) //右まわりか左まわりかで、モータのCWCCWを切り替える
{
case front:
case rear:
return; //超信地旋回なので、前や後ろは無い.
case right: //右に回るように設定
MOT_CWCCW_R = MOT_BACK;
MOT_CWCCW_L = MOT_FORWARD;
break;
case left: //左に回るように設定
MOT_CWCCW_R = MOT_FORWARD;
MOT_CWCCW_L = MOT_BACK;
break;
}
//最低速度を設定
if(tar_speed < MIN_SPEED) { tar_speed = MIN_SPEED; }
r_accel = TURN_ACCEL; //回転の加速度を決定
max_speed = TURN_SPEED;
min_speed = MIN_SPEED;
speed = MIN_SPEED; //速度をリセット
step_r = step_l = 0; //ステップ数をリセット
obj_step = LEN2STEP(times*TREAD_CIRCUIT); // 目標ステップ数算出
wait_ms(250); // 少し待機
MTU.TSTR.BIT.CST3 = MTU.TSTR.BIT.CST4 = 1; // カウントスタート
//減速開始すべき位置に達するまで待つ
while( ((times*TREAD_CIRCUIT) - STEP2LEN(step_r + step_l) ) >
( ((tar_speed*tar_speed) - (speed*speed) ) / (-2*1000*TURN_ACCEL) ));
r_accel = -TURN_ACCEL; //減速開始
while((step_r + step_l) < obj_step) ; //停止する位置に達するまで待つ
//モータ停止
MTU.TSTR.BIT.CST3 = 0; //モータのカウントをストップ
MTU.TSTR.BIT.CST4 = 0; //モータのカウントをストップ
//ターン後少し待つ
wait_ms(250);
}
と実装されています。
ここまでの関数で出てきたパラメータは、直進と回転の速度と加速度くらいです。
結局、安定した動きを生成実現するには、割り込み処理の関数 int_cmt0、int_cmt1を調整する必要があることがわかりました。
int_cmt0関数の中では、壁沿い制御のための制御変数 con_wall.kp くらいであり、int_cmt1の中の壁を発見する変数 sen_r.th_wall, sen_l.th_wall, sen_fr.th_wall, sen_fl.th_wall 、壁沿い制御開始の閾値となる sen_r.control, sen_l.control
および制御目標値の変数 sen_l.ref, sen_r.ref になります。
これらの変数は、init.cのinit_parameter関数内で下のように代入されています。
// 壁沿い制御のためのリファレンス値の初期化
sen_r.ref = *(unsigned short *)REF_SEN_R_ADD; // 右センサ
sen_l.ref = *(unsigned short *)REF_SEN_L_ADD; // 左センサ
// 壁有無判断の閾値の初期化
sen_r.th_wall = *(unsigned short *)TH_SEN_R_ADD; // 右センサ
sen_l.th_wall = *(unsigned short *)TH_SEN_L_ADD; // 左センサ
sen_fr.th_wall = *(unsigned short *)TH_SEN_FR_ADD; // 右前センサ
sen_fl.th_wall = *(unsigned short *)TH_SEN_FL_ADD; // 左前センサ
// 壁沿い制御をかけるか否かの閾値の初期化
sen_r.th_control = CONTH_SEN_R; // 右センサ
sen_l.th_control = CONTH_SEN_L; // 左センサ
調整すべき変数がわかったところで、これらの変数を機体に合わせて変更していきます。
上記の変数をよく見ると XXX.th_wallで設定されている壁を検出するための閾値は、迷路内の様々な区画の間（半区画進んだ場所）に置いてみて壁があるときとないときのセンサ値から決定すれば良さそうです。
また、制御目標のリファレンス値は、両方に壁がある場所でセンサの値を測定して決定すればいいことがわかります。
社内にある迷路で上記のセンサ値を測定して、下記のようにパラメータを決定しました。
# define REF_SEN_R 600 # define REF_SEN_L 668 # define TH_SEN_R 240 # define TH_SEN_L 250 # define TH_SEN_FR 173 # define TH_SEN_FL 185
これらの値を設定して、５ｘ５の迷路を走らせた結果、無事にゴールまで到達することができました。
ちなみに壁沿い制御のための制御変数 con_wall.kp は、デフォルト（0.3）のままでしたので、制御が強すぎる感じがしました。
あとは、１６ｘ１６の迷路での走破テストになりますが、続きは次回にします。