キット製作研修マイクロマウス研修（hara）
Pi:Co Classic3在宅研修

マイクロマウス研修(hara)その6

キット製作研修

こんにちは。haraです。

前回は、スラロームの実装を行ってみましたが、理論通りではうまくいきませんでした。旋回速度のパラメータをいじったら少しはましになったので、これを左手法の探索の部分に適用して動作を見てみます。

スラローム走行を左手法の探索に使う

前回実装したスラローム走行は、迷路の一マス全体をカーブ

する実装でした。
これを左手法の探索部分に組み込んでみます。

スラロームを行う関数は、turn_slalom_r()という名前にして下のように実装しています。

void turn_slalom_r(t_local_dir dir)
{
  if (dir == front || dir == rear) return;
  float turn_sp=SEARCH_SPEED;
  int turn_sp_diff = speed * 0.37; // 0.411 * 0.9
  int obj_step;

  r_accel=0;
  con_wall.enable = false;
  obj_step = LEN2STEP(PI*45);

  switch(dir){
    case right:
      add_speed = turn_sp_diff;
      break;
    case left:
      add_speed = -turn_sp_diff;
      break;
  }
  step_r = step_l = 0;
  MOT_CWCCW_R = MOT_CWCCW_L = MOT_FORWARD;
  MTU.TSTR.BIT.CST3 = MTU.TSTR.BIT.CST4 = 1;

  while((step_r + step_l) < obj_step);
  MTU.TSTR.BIT.CST3 = MTU.TSTR.BIT.CST4 = 0;

  con_wall.enable = true;
  add_speed=0;
  return;
}

この関数では、マスの中央を走行することを前提にしていますので、中心線が PI*(HALF_SECTION/4) 進むまで左右の速度差を付けるようにしています。

スラロームの速度差は、理論的には 37/(回転半径）でいいはずですが、前回のブログでのテストの結果10％だけ減じています。このスラローム走行を左手法の探索に実装した結果は、下の動画のようになりました。

結果的には、開始早々のS字カーブを曲がることがなかなかできなくてうまくいきませんでした。
そこで、スラローム走行を少し改良してみます。

スラローム走行の半径を調整できるようにする

右（左）折のスラローム走行で、一マス全部でターンするやり方は、単純なパラメータ調整では無理のようですので、スラロームの半径を調節できるようにします。

まずは、理論的に考えてみます。
ターンする一マスは、180×180の正方形になっていますので、中央で半径 r のターンをすることを考えてみます。右折のターンを考えてみると下の図のようになると思います。スラローム走行の半径

従って、半径rでターンする場合には、まず距離d(=90-r)だけ直進し、そのあとに半径rで90度ターンし、距離dだけ直進すれば良いようです。

直進する距離は、ターンする半径で決まりますので、あとはターン時の走行時間と左右のタイヤの速度差を決めてあげれば良いようです。

ターン時の走行時間は、ロボットの中央の走行距離で定義できますので、モータに与えるステップ数は、LEN2STEP(PI*r/2.0)となります。

残るパラメータは、左右のタイヤの速度差ですが、理論的には (ターン時の速度）ｘ （タイヤ間の距離/2）/ (ターン半径）となりますが、
実際の走行では、調整が必要な場合がありますので、変数にして実装すると下のようになります。

void turn_slalom(t_local_dir dir, float turn_sp, int r, float diff_param)
{
  if (dir == front || dir == rear) return;
  int dist = HALF_SECTION - r;
  float init_sp=speed;

  // Move straight
  straight(dist, init_sp, turn_sp, turn_sp);

  int turn_sp_diff = turn_sp * diff_param; // turn_sp * 37 / r
  int obj_step;

  r_accel=0;
  con_wall.enable = false;
  obj_step = LEN2STEP(PI*r/2.0);

  switch(dir){
    case right:
      add_speed = turn_sp_diff;
      break;
   case left:
      add_speed = -turn_sp_diff;
      break;
  }
  step_r = step_l = 0;
  MOT_CWCCW_R = MOT_CWCCW_L = MOT_FORWARD;
  MTU.TSTR.BIT.CST3 = MTU.TSTR.BIT.CST4 = 1;

  while((step_r + step_l) < obj_step);
  MTU.TSTR.BIT.CST3 = MTU.TSTR.BIT.CST4 = 0;
  con_wall.enable = true;
  add_speed=0;

  // Move straight
  straight(sidt, trun_sp, init_sp, init_sp);
  return;
}

これでターンの半径を調整できるスラロームの実装ができました。上述のスラロームの実装と同じように、左手法の探索に組み込んでテストします。

最初にスラロームの走行半径を85㎜に設定して、左右の速度差のパラメータを0.4に設定しています。

この場合には、R90でスラロームを行ったよりも、少しはましになりましたが、まだまだです。
もう少しターンの半径を調節してみてテストします。
今度は、スラロームの走行半径を80㎜に設定して、左右の速度差のパラメータを0.4に設定しています。

ターン半径が80㎜だとかなりうまくいきました。しかし、よく見ると直進する部分をもう少し伸ばすと良さそうなので、プログラムをしたのように改変しました。

void turn_slalom(t_local_dir dir, float turn_sp, int r, float diff_param, int d)
{
  if (dir == front || dir == rear) return;
  int dist = HALF_SECTION - r + d;
  float init_sp=speed;

  // Move straight
  straight(dist, init_sp, turn_sp, turn_sp);

  int turn_sp_diff = turn_sp * diff_param; // turn_sp * 37 / r
  int obj_step;

  r_accel=0;
  con_wall.enable = false;
  obj_step = LEN2STEP(PI*r/2.0);

  switch(dir){
    case right:
      add_speed = turn_sp_diff;
      break;
   case left:
      add_speed = -turn_sp_diff;
      break;
  }
  step_r = step_l = 0;
  MOT_CWCCW_R = MOT_CWCCW_L = MOT_FORWARD;
  MTU.TSTR.BIT.CST3 = MTU.TSTR.BIT.CST4 = 1;

  while((step_r + step_l) < obj_step);
  MTU.TSTR.BIT.CST3 = MTU.TSTR.BIT.CST4 = 0;
  con_wall.enable = true;
  add_speed=0;

  // Move straight
  straight(sidt, trun_sp, init_sp, init_sp);
  return;
}

このプログラムで、ターン時の半径80㎜、直進部分を　⁺5㎜にしてテストしました。

この動画をみると、やはり少し直進部分を追加した方が良さそうです。

何とか１周回れるくらいにはなりましたので、この実装を元にして高速迷路走行の部分に追加していきたいと思います。

この動画中でも、壁への衝突や、180度旋回時にズレが生じる場合もありましたので、センサの調整なども必要ですね。

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