目指せ全日本大会完走 – Yoのマイクロマウス研修　Part.6

こんにちは。Yoです。

前回はスラローム走行の実装についてでした。

スラローム走行を実装したことで走行時間が大幅に短縮されたものの、探索が長引くと
蓄積された機体のズレにより壁と衝突してしまうことが多々ありました。

そこで今回は「前壁を用いた機体位置補正」について書いていこうと思います。

機体位置補正とは

機体の直進や左右旋回、180°旋回時にスリップや計算値の誤差から位置ズレが発生すること
があります。

この記事では、機体の位置ズレ修正動作を「機体位置補正」とします。

機体位置補正方法の選定

機体位置補正方法として機体後部を壁に当て壁と垂直にする方法が多く見受けられましたが、
「Pi:Coはステッピングモータだから真っ直ぐにはなるが前後距離はズレる危険性がある」
と教えていただいたので前後距離も調整できる別の方法に挑戦することに。

地区大会で見かけた他の補正方法としては前壁を認識 → 一区画後退 → 前進しながら左右距離と
前壁距離を調整というものもありました。

機体位置補正のやり方が多数あることを知った上で、自身のスラローム走行では旋回動作後に
車体の角度が大きくズレないことから180°旋回時に前壁との距離のみで位置補正を行う方法を
選択しました。

これだけでは左右距離の補正ができないので、180°旋回を2回の90°旋回に分けることにより
左右距離も調整し機体を区画中央へ補正します。

イメージとしてはこんな感じになります。

実装

続いて実装パートです。相も変わらず段階を踏んでいきます。

① 前壁との距離を調整するプログラムの実装

↓

② 180°旋回を90°旋回2回に分ける

↓

③ 壁補正を行うかどうかの判断条件を設ける

① 前壁との距離を調整するプログラムの実装

まずは前壁との距離を調整できるように実装していきます。

前壁に対して距離が遠かったらゆっくり直進し、近すぎたらゆっくり後退という動作を
無限ループさせます。

      controlInterruptStop();
      g_speed = 0;
      g_omega = 0;
      g_min_speed = 0;
      g_max_speed = 500;

      g_con_wall.enable = false;
      controlInterruptStart();
      g_speed = 100;
      g_motor_move = 1;
      while(1){
      moveDir(MOT_FORWARD,MOT_FORWARD);
      g_speed = 0.02 * ((REF_SEN_FL + REF_SEN_FR) - (g_sen_fr.value + g_sen_fl.value));
      g_omega = 0.01 * ((REF_SEN_FL - REF_SEN_FR) - (g_sen_fr.value - g_sen_fl.value));

      delay(2000);
      moveDir(MOT_BACK,MOT_BACK);
      delay(2000);
      }
      g_motor_move = 0;

これで機体の前に壁を置き近づけたり遠ざけたりするとその距離に合わせて
一定の距離を取り前後するようになります。

続いて、前壁との距離が適切な距離になったら停止するように脱出条件を追加します。

void wall_correction()
{
  controlInterruptStop();
  g_omega = 0;
  g_accel = 0;
  g_omega_accel = 0;
  g_min_speed = MIN_SPEED;
  g_max_speed = SEARCH_SPEED;

  g_con_wall.enable = true;
  controlInterruptStart();
  g_motor_move = 1;
  while(1){
    int position_error = ((REF_SEN_FLC + REF_SEN_FRC) - (g_sen_fr.value + g_sen_fl.value));
    if(-10 < position_error && position_error < 10){
        break;
    }
    g_speed = 0.1 * position_error;
  }
  g_motor_move = 0;
  delay(250);
  controlInterruptStop();
  g_speed = 0;
  g_omega = 0;
  g_accel = 0;
  g_omega_accel = 0;
  g_min_speed = MIN_SPEED;
  g_max_speed = 350;
}

最終的な停止位置はシリアルモニタでセンサ値を見ながら目視で調整した大雑把な
区画の真ん中あたりになります。

上記のハイライト部分に脱出条件用の新たなパラメータとしてREF_SEN_FLC と
REF_SEN_FRCを追加しました。

この2つのパラメータへ上記停止位置の前壁センサ値を入れ調整します。

② 180°旋回を90°旋回2回に分ける

前壁との距離を調整することに成功したので次は90°旋回後も同じ動作を行えるようにします。

順序としては、位置補正 → 90°旋回 → 位置補正 → 90°旋回といった動作になります。

これは書き換える箇所がほぼないので詳細は割愛しますが、パラメータに設定したセンサの
目標値によっては壁に近すぎた際にセンサ値が一時的に下がり機体が壁まで遠いと誤認し
壁に衝突してしまうことがあります。

どうやら距離が近すぎると光センサへ正確に光が反射してこないようです。

この状態への対策は結局できずに終わりました。

③ 壁補正を行うかどうかの判断条件を設ける

現在のプログラムのままだと180°旋回時に必ず位置補正を行なってしまいますが、
迷路探索中は袋小路区画ではない区画での180°旋回も存在します。

その場合にこのプログラムのままだと補正するための前壁を見つけるまで進んでしまうので、
次のプログラムを追加し袋小路区画の時のみ補正をするようにします。

void wall_correction_judge()
{
  if((g_sen_l.is_wall == true) && (g_sen_r.is_wall == true) && (g_sen_fr.is_wall == true)){
    wall_correction();
    rotate(right,1);
    wall_correction();
    rotate(right,1);
  }
  else{
    decelerate(HALF_SECTION, SEARCH_SPEED);
    rotate(right,2);
  }
}

3行目で前壁と左右壁を見て判断しています。

欠点

この前壁を用いた機体位置補正にも欠点はあります。

あくまでこの補正プログラムは前壁との距離を調整するだけなので、
機体が大幅に斜めを向いていた場合は姿勢を直せません。

前壁補正を過信せずセンサ調整と旋回調整を行うことが重要です。

次回予告

次回は初出場の全日本大会結果について書いていこうと思います。