物理シミュレーションを行うときには、必ず物理方程式の離散化を考えますが、今回は軽量シミュレータなので、運動方程式といっても一階微分方程式くらいのもんなので、楽勝です。
微分方程式は、漸化式による差分方程式を作ってシミュレーションしますが、ここで注意しなくてはならないのは、位置を更新する際の経過時間Δtです。
Δtは小さいほど精度良くシミュレーションできますが、このシミュレータでは、パスファインダーに入るセンサー情報のサンプリング周期とΔtを同期させている為、Δtはシミュレータのサンプリング周期にあわせる必要があります。
Δtが100msecとか、あんまり大きいとまずいなと思い実機でサンプリング周期をはかってみたところ5msecでした。
最高速度500mm/s程度のスケール感なら、まあいいかということで、Δtを5msecにしています。あとはこの状態でシミュレーションをして実機との挙動があまりに違っていたらΔtの長さの変更や、Δtとシミュレータのサンプリング周期の同期をやめるなどの判断をしていきたいと思います。
これを私の得意技現物あわせといいます。
コースは1mmにつき1pixelとして取り扱うことにします。つまり1m×1mだと1000pixel x 1000pixelのコース画像を用意することになります。
パスファインダーの先端部分にある光センサーの座標を計算し、その座標に対応するコース画像上の1pixel x 1pixelの色情報を取得して、LightSensorの値にセットするというシンプルなロジックです。
ということで、(日記にはあまり書いていませんが)紆余曲折を経てシミュレータ部分のコーディングがなんとか終わりました。
UTは共通部分のみ。
ねんのためリバースかけてみました。