ベクトル

id:ohguma:20150602 で紹介したボールセンサのアイデアを検討してみる。

対象はTJ3Bの３輪オムニのこれ。ボールセンサは前後左右の4つ。

前センサを起点に時計回り（CW:clockwise)に1〜4の番号を割り当てる。各ボールセンサは90度ごとに外向きに配置しており、それぞれを長さ１（半径１の円上）の単位ベクトルとして表す。
前センサを基準に時計回りの角度を計算するために、前センサをＸ軸＋、右センサをＹ軸＋の向きにする。

ボールセンサは「電子工作関係: 赤外線リモコン受信モジュールについて１」のコメントにあるTSOP4038にローパスフィルタ(R:39KΩ、C:33uF(334))を組み合わせたものを使う。
出力はC-Style上ではボールがない場合が1023、近いほど小さな値となる。

計測値をボールがなければ0、近いほど大きな値となるよう変換して使う。
計測値を単位ベクトルに掛け、各センサの信号の強さをベクトルで表す。
（下の4式のCNxはCNx'の誤記）

全センサのベクトル値（X,Y)を足して、合成したベクトルVallのX,Y座標からatan2()で求めた角度がボールのある向きとなる。
atan2()の結果はX=0,Y=0の場合は処理系の実装に依存するので、X=0,Y=0の場合はatan2()を使わないようにする。