座標系

物理量ベースの考え方でマイクロマウスのプログラムを組んでいる場合、座標系をしっかりと決めておくと便利な場合があります。

例えば、次のような場合で応用が考えられます。

• 迷路探査時、マス(の切れ目)に囚われない処理の起点として
• ログとして記録しておくと、怪しい動きをする場所がわかる
• 賢い壁切れ補正
• シミュレーションによる最短(距離)経路の計算
• シミュレーションによるスラロームの自動生成
• 他にもアイデア次第でいくらでも

まずは単位系を決めましょう。各々使っている距離の単位([mm]か[m])を使うと後からすんなり計算できるようになります。 ここではmmで説明します。
次に、原点を決めます。例えば、マス目としての座標(0, 0)の左下の柱の中心を原点としてみます。
後は、X軸、Y軸をどう取るかを決めれば座標系が完成します。今回は、マウスがスタートから最初に進む方向をY軸、そこから右方向をX軸としてみます。右手系です。
ここまで決めると、マス目座標(2, 1)の区画の中心の座標は(450.0, 270.0)というように座標を決める事ができます。

前節までで、迷路上の点を定義する準備ができました。
しかし、マウスが自身で持っている情報を考えてみると、(基本的には)速度と角速度です。

物理量ベースで走っている人には想像がついていると思いますが、座標をリアルタイムに更新するには
speed*Δtを積分していくことになります。今持っている情報だけでX,Yの座標を更新しようとすると、speedをxとyに分解して持たなければなりません。
これは多分面倒くさい事になるので、やりたくありません。

角度の情報を持っておくと、簡単に解決できます。

speed_x = speed * cos(angle);
speed_y = speed * sin(angle);

とすることで、迷路上でどう進んでいるかを表現できます。
上に示したプログラムのように実装するのであれば、+X方向に0[rad]を取り、左回りに角度を決めた事になります。

最初にマウスを置く位置でx,y座標を初期化、角度をPI/2で初期化し、角度と座標をそれぞれ積分することで実装できます。
ただし、いつもの積分法は誤差が蓄積するので、適当にリセットを行った方が良い。