モーターノイズの正体

加速度センサを実験用ロボのシャーシに取り付けました。ちょっと傾いてもまずいだろうということで、ビスナットでしっかりと固定しました。

ソフトはジャイロコンパスと同じものを使い、まずは１回積分で速度を表示させてみます。
静止状態では速度０で安定してますが、駆動輪が空転するようにしてモーターをオンにすると、たちまち表示がばらつき始めます。ロボットは移動していないので、これはモーターノイズです。

予備実験の様子から、おそらく機械的振動だろうと、加速度センサのブレットボードを取り外して、安定な場所におきました。
ノイズが無くなり、０で安定になりました。あたりです。緩衝材をかませて取り付けなければならないようです。

ここでひらめきました。そう、ジャイロコンパスに紛れ込むモーターノイズです。
ジャイロセンサは圧電効果を利用しています。と、いうことはセラミックマイクのように機械的振動にも敏感ということではないでしょうか。
早速、ビスで固定していたジャイロ基板を外して、ティッシュペーパーを軽くたたんだクッションの上に置いてテストしました。

結果良好です。モーター回転時にジャイロがふらつくことが無くなりました。
どうやらモーターノイズの主要因は電気的なノイズではなかったようです。

加速度センサの予備実験

加速度センサの出力を増幅して波形を見てみます。
非反転増幅器で10倍に増幅してみました。回路は小さなブレットボードに組んであります。

これを静かに動かして出力波形をオシロで読みます。
ゆっくりボードを滑らせても敏感に信号が出ます。かなり小さな加速度でも検出できるようです。

アーそうかと思ったのは、マイクロホンのように近隣の振動を拾うことです。ブレットボードに円盤形のセラミックコンデンサが見えると思いますが、これはローパスフィルタの時定数です。これがないと、手を叩いた音で出力が出ます。
この状態でも振動には敏感です。この写真は、ブレットボードが載っているテーブルを軽くたたいたときの出力です。

正負方向に均等に振れるノイズなら積分すれば０になりますが、この信号はどう処理されるでしょう。

次は実験用ロボに搭載してテストしてみます。

ひさしぶりのSFプラモ

【ロボの一言】気になる感じデス

最近「ロボット作ろう」ばかりでネットタンサーウェブの相手をすっぽかしていました。相変わらずナイスなコメントありがとうございます。

このイマイのアキレスは、モーターの回転軸をこまの心棒のように床に斜めに当て、ギアなしで走行するという画期的なメカでした。しかもその回転軸にプロペラをつけ、あたかもホバークラフトのようなイメージにしているのです。

私は小学校６年生くらいの時にコレを作っているのですが、そのとき大失敗をした思い出のモデルなんです。

そういう特殊な駆動なので、モーターは正面から見て少し斜めに取り付けられます。しかし、私は当然、ホバークラフトのようにファンで浮上するものと思っていましたから、プラバンを削ってモーターがまっすぐにつくようにしてしまいました。自分の買ったキットが不良品だと思ってしまったのです。当時のプラモはそんなことはよくあることでしたから。

当然、浮きませんし走りません。

今回、リベンジで入手し、なんだかほっとしています。人生のやり直しというと大げさですが、これで積年のもやもやが晴れた気分です。

加速度センサーを検討しよう

今回の予備実験の目的は、加速度センサーでロボットの走行距離を計測することです。
車輪のトリップカウンタでいいじゃん、とは思いますが、モノは試しです。

センサーは毎度のことながら秋月のこれにしました。最近の部品は面実装ばかりなのでモジュール化してくれると助かりますね。

さて、加速度センサー要求性能はどんなもんでしょう。
ロボットの速度は100mm/secくらい、２秒ほどで最高速度になるとすると、平均加速度は50mm/s2になります。
センサーのデータを読むと5V使用時には1000mv/Gですから、G=9.8m/s2=9800mm/s2、50mm/s2のときの出力は5.1mVしかありません。
うーん、ADの分解能は4.9mVだから、そのままでは使用できません。OPアンプで増幅する必要があります。少なくとも10倍、安全を見込んで20倍くらいのゲインは必要です。
これで電気回路の設計方針はできましたが、問題点もあります。そもそも1/1000Gくらいの加速度をこのセンサーが感知できるのでしょうか？温度ドリフトは？（データシートには±100mVなんて恐ろしい値が書いてありますが、/℃というわけでは無いようです）まあ、あまり考えずにやってみることにします。

加速度の情報を積分すると速度が得られます。速度を積分することで距離になります。高校のときに習った物理が役に立ちました。先生ありがとう！
積分は2msごとに加速度、速度の成分を積算することで数値計算的に行います。
ソフトの設計方針はこれでいきましょう。

シェーキーのモックアップを作りました

ジャイロが一段落したところで、ジェーキーに戻りましょう。

いろいろと細部を検討するためと大きさの感じをつかむため、モックアップを作ることにしました。

21世紀の常識では3D-CADを使ってモデルを作成するのですが・・・カメラやセンサなど部品のおさまり具合を見るにはその部品もモデル化しなければなりません。一品だけ作るにはちょっと工数が多いように思います。

ということで、今回は「模型とラジオ」方式でやりましょう。写真から寸法を割り出し、紙でモックアップを作ります。

ネットで写真は手に入りますが、あまり種類は無いようです。そのうえ、いろいろと改造されてきているので、アンテナや頭部の取り付け位置、付属機器に違いがあります。
ともあれ、寸法割り出し用には引退時と思われる正面と側面の写真を使います。駆動輪が直径65mmと言う条件で各部の寸法を出していきます。駆動輪に使うつもりのアルミプーリーがこの寸法だからです。

製図道具が無いので、フリーウェアの2D-CAD、jwcadを使って寸法をあたりました。初めて使いましたが、一昔前のCAD専用機風で慣れると使い易いです。（タブレットで入力するようなヤツです）

外形の寸法を決めたら、工作用紙（懐かしい！）に写してカッターでざくざく切り出します。楽しい！結構な分量の工作用紙が必要でしたが、おなじみダイソーで５枚100円なので問題なしです。テープとセメダインコンタクトで組み立てたのがこれです。

モデルは良く見かける斜めから撮ったシェーキーです。比べるとこんな感じです。

カメラの位置がちょっと高いのと、頭部の幅がもう少し広い方がそれらしいかもしれません。多少デフォルメが必要です。
胴体はちょっと大きめに見えますが、これは最大部分の寸法なので、実際はもう少し小さくなるでしょう。また、白は膨張色なのも影響があります。

全高60センチ、スケールで言うと1/3スケールというところでしょうか。
ネットタンサーのカメラ基板をあてがってみると、カメラ内部に十分に収まります。この要領で、頭部に取り付け予定の各種センサーも現物合わせで取り付け位置を決めればいいでしょう。
カメラの地上高は45センチほどです。猫を追いかけるにはちょっとばかり低いかもしれません。ウチの猫は高いところで昼寝をするのが大好きなもので。

こうやって実物大モックを作ると、細部の仕様決めに便利なだけでなく、ヨシやるぞ！と言う気分になれます。最後にはゴミになっちゃいますが、作ってよかったようです。

ジャイロコンパスまとめ（その２）

実験用ロボのここまでの回路図はこんな感じです。クリックでPDFを表示します。ただし、現在はブレットボードのノイズ対策のためPSDへの電源供給はカットしてあります。

「091120.pdf」をダウンロード
　
　
次はPICのプログラムの解説です。

ジャイロコンパスの更新部分です。この処理はタイマー割り込みでジャイロセンサのAD変換のたびに実行されます。周期は2msです。　
グローバル変数とその内容は次の通りです。longは16bit、long longは32bitです。

signed long long　　gyro_compass;　　ジャイロコンパスです
signed long long　　gyro_ma[10];　　　移動平均用の履歴を保存します
signed long long　　gyro_ma_result;　作業用の変数です
signed long long　　gyro_work;　　　　これも作業用の変数です
signed long　　　　gyro_initial;　　　　無信号時のセンサの値です
中にはグローバルでなくても良さそうなのもありますが、現在はこうなっています。

これが処理の本体です。

//直近8回分の計測値の記憶（正直にやるとこうなる）
gyro_ma[0] = gyro_ma[1];
gyro_ma[1] = gyro_ma[2];
gyro_ma[2] = gyro_ma[3];
gyro_ma[3] = gyro_ma[4];
gyro_ma[4] = gyro_ma[5];
gyro_ma[5] = gyro_ma[6];
gyro_ma[6] = gyro_ma[7];
gyro_ma[7] = adbuff[9];　　//adbuff[9]にはジャイロセンサのAD変換値

//直近8回分の移動平均の計算
gyro_ma_result = 0;
for(i = 0; i < 8; i++){
　　gyro_ma_result += gyro_ma[i];
}
gyro_ma_result = gyro_ma_result >> 3;　　//8で割る

//方位角の計算
gyro_work = gyro_ma_result - gyro_initial;　//変化分の計算
if((gyro_work > 1)||(gyro_work < -1)){　　　//閾値の設定
　　gyro_compass += gyro_work;　　　　　//コンパスの変更
}

つまり、2msごとに直近８回分の測定結果の平均値を取り、これを無信号時（静止時）のジャイロセンサの値と比較して、閾値を超えたら、その差分（正負）をコンパスの値に足し込むという操作をしています。
　
　
コンパスと無信号時のジャイロセンサの初期化は次の関数で行っています。この関数はロボットが静止しているときに実行します。

//ジャイロコンパスの初期化
//静止時の出力（基準値）を設定し、コンパスを０にリセットする。
void cal_gyro(void){
　char i;
　signed long xx;
　signed long long xxxx;

　//基準値を設定→その値でのドリフトを計測をドリフトが一定値に収まるまで繰り返す
　//注意！！無限ループ
　while(1){
　　//ジャイロスターの出力を5ms間隔で20回読んで平均を取る
　　xx = 0;
　　for(i= 0; i < 20; i++){
　　　xx = xx + adbuff[9];
　　　wait_proc(5);　//5ms待つ
　　}
　　gyro_initial = (xx / 20); //ジャイロスターの基準値を計算

　　//ドリフトを計測し一定の範囲内なら終了
　　xxxx = gyro_compass; //現在のコンパスをセーブ
　　wait_proc(500); //500mS待つ

　　if(gyro_compass > xxxx){
　　　//右回りのドリフト
　　　if(gyro_compass - xxxx < 20) break; //ドリフトが許容範囲内なら無限ループ抜ける
　　　}
　　else{
　　　//左回りのドリフト
　　　if(xxxx - gyro_compass < 20) break; //ドリフトが許容範囲内なら無限ループ抜ける
　　}
　}
　gyro_compass = 0; //コンパスを０にリセット
}

この関数は、無信号時（静止時）のジャイロセンサの値を20回測定して平均を取り、gyro_initialに基準値として保存します。次にこの基準値を使ってコンパスを500ms作動させ、ドリフトが許容範囲内かどうかを調べます。OKならコンパスを現在の方向が0としてリセットし初期化終了です。ドリフトがあるようならもう一度やり直します。
コードの中にはコンパスの処理がありませんが、前に説明したように、コンパスはタイマー割り込みで処理されているので、待っていれば結果は出ます。

現在はこのような素朴なコードで動作しています。

ジャイロコンパスのまとめ

ノイズ取りのため、コンデンサを入れたり配線の引き回しを変えたりした結果、なんとかジャイロコンパスの方位を使って、そこそこナビゲーションすることが出来るようになりました。
前進距離は時間で指定していますが、前進方位と旋回角度はコンパスで決めています。エンコーダなしとしてはまあまあの結果でしょう。
当然ですが、途中でカーペットに乗り上げたりしても正確にコースをトレースできます。（前進距離は変わってきますが）

https://youtu.be/VWHliHJfpMc

ジャイロコンパスの値が0で北、47000で東、-47000で西、94000で南としてナビゲートしています。まだモーターノイズを取りきれていないので、場合によってはもう少し軌道がずれます。

軌道がずれるのは、ジャイロコンパスがジャイロセンサの出力に重畳したノイズでずれていくためです。

その他は思った通りに動いているので、ひとまずジャイロコンパスの実験を終了します。

次は加速度センサで移動距離を計測してみたいと思います。

ジャイロコンパスのノイズはどこから？

見たところジャイロの出力にノイズが重畳しているのはわかりました。ただ、実際のところどれくらい影響が出ているのか調べてみます。

実験用ロボのジャイロコンパスは、C#で作ったコンソールからリアルタイム（100mS周期）で読み出すことが出来ます。

画面左下のCOMPASSにはジャイロコンパスの方向を表す数値が表示されます。左のリセットボタンをクリックすると０になり、右回りで＋、左回りで−の数値になります。この数値はジャイロコンパスの数値を1/100にして小数点以下１桁まで表示させたものです。
写真の状態はリセットから右回りに手動で90度ほど回転した状態です。470くらいで90度（つまり内部数値では47000）です。左回りでは-470くらいです。
その右隣のGYROはジャイロセンサのAD変換値です。単位は4.9mVです。

テストしてみると、モーターを回していないときでも、ジャイロコンパスの数値があまり安定でない感じがします。30秒で50くらい変化することもあります。また、全く変化しないときもありでどうもノイズっぽいです。

そこで、ああ、これかもとPSDへの給電をやめてみました。安定です。かなりパルス的に電流を食っているらしくこの辺が不安定な問題の一因かと思われます。

これで静止時にはかなり安定になりました。リセット後のコンパスの数字は0からほとんど変わりません。ただし、数分するとドリフトが始まるのか変動してきます。ともかく短期的には良さそうです。

そしてモーターです。
モーターノイズはまだ取れません。安定になったジャイロコンパスを読むと、モーターが回転時にはコンパスがマイナス方向にずれていくようです。
だとすると相対的にプラス方向（右回り）のオフセットが働くので、クランク走行の時のような誤差が生まれるのかもです。

電気的なノイズは、残念ながらブレットボードではきちんと対応するのが難しいようです。モーターノイズも電源から回り込むのか、はたまた配線からか、機械的振動なのか、モーターからなのか、過電流気味のドライバの保護回路からなのか・・・このへんちゃんと対応するには、ユニバーサル基板で引き回しに気を使った回路を組んだ方がよいでしょう。

ちょっと考えましょう。

ジャイロコンパスの問題点を考える

ジャイロコンパスを使ってみると、左右の回転角に誤差があることがわかりました。

右９０度、左９０度とクランク走行させてみると、左回転の角度が足りないようです。この原因はなんでしょう。
まず、考えられるのが、ソフトの問題です。

試作ソフトでは回転角度は現在の進行方向に対しての相対角度で与えられます。つまり、回転直前の姿勢がずれていれば、その分旋回角度もずれるわけです。
クランク走行では、動画でもわかるよう、進行方向に対してフィードバックがかかっているため、頭を振りながら進行します。そのため、旋回直前にぴったり進行方向に向いているかどうかは怪しいものです。

次に考えられるのは、ジャイロセンサへのノイズの混入です。

モーターを駆動していないときには問題ありませんでしたが、モーターを駆動してみると、数マイクロ秒くらいの細かいパルスがたくさん入ります。
思っていたより相当多い。これは問題です。200mV/div 1mS/divで測定するとこんな感じです。デジタルオシロなのでノイズの幅が相当広いように見えますが、実際は数マイクロくらいです。
サンプリング周期が2msなのですべてのノイズが読み込まれるわけではありませんが、ノイズの電圧が大きいのは問題です。ADは直近８回分の信号の平均を計算しているので、大きなノイズは16msにわたって影響を及ぼします。ちょっとまずいですね。アルゴリズム的に測定値のフィルタリングの必要がありそうです。

本来は、センサの回路にノイズが混入しないよう、電気的なデカップリングの必要がありますが、ブレットボードのバラック配線では、きちんとしたノイズ対策はできません。とりあえず、ADコンバータの入力にパスコンを入れてみるつもりです。

さらにもうひとつ悪いことが。このノイズはモーターから出ているのですが、電流が想定よりずっと多く、モーター二つをデューティ70%位で駆動すると600mA位も電流を食っているようです。かなり発熱していてサーマルシャットダウンがかかり気味という感じです。
コレはまずい。モーターをもっと電流の少ないものに変えるか、ドライバをパラで使うほうかいいでしょう。

さしあたりソフトPWMをモーターON→ブレーキからモーターON→モーターOFFに切り替えました。ブレーキ分の発熱がないだけよさそうです。
なお、PWMは64mS周期で分解能200uS、３２段階でソフト的に作成しています。おもちゃのモーターなのでコレくらいでうまくいきます。

サンプリングや相対角度の問題は、もうすこし考えてから対策してみようと思います。

とりあえずジャイロで動かしてみます

とにかく簡単なジャイロコンパスのソフトを作りました。

無信号時のジャイロの読み出し値を０として、2mSごとにこの基準値からの偏差を積分します。ソフトは後ほどアップしますが、これでロボットを動かした動画がこれです。

https://youtu.be/TEgJ2pZqBJ8

３秒前進、右旋回９０度、前進３秒、左旋回９０度、前進３秒、停止のシーケンスを実行します。左右の旋回には同じパラメータを与えていますが、なぜか？何回やっても動画のように角度が多少異なります。

前進はやはりジャイロで進行方向補正をかけてあります。車輪のエンコーダはもとよりありません。

とりあえず動いたということで、いろいろ具合の悪いところをこれから検討していきたいと思います。

ジャイロ実用回路をテストする

実用回路をこのように設計しました。

オフセット電圧の生成には手持ちの1.8Vの三端子レギュレーターを使いました。毎度のことながら秋月で買ったものです。
回路は前の説明した通りですが、1.8Vレギュレータの負荷に47オームの抵抗を入れ、38mAほどの電流を流してあります。これは、レギュレータのOUTにはジャイロセンサの動作電流が流れ込んでくるので、この電流をパイパスするためと、電力用のレギュレータは最低20mAくらいは電流を取り出さないと安定にならないからです。モッタイナイですがこれくらいは電流を流さなければなりません。

ところでジャイロスターのデータシートには消費電流もないですね。手持ちのデバイスでは実測で1.5mAくらいでした。

これを前の実験と同じようにロボットを旋回させて計測してみました。前回と異なるのは、ロボットが１秒ほど右旋回、0.5秒ほどの休止、１秒ほど左旋回で上下の波形を表示させているところです。チャンネルAがセンサの出力で500mV/div、チャンネルBがモーターの駆動マーカーです。時間軸は500mS/divです。

無信号時の電圧は約2.3V。前回計測したときに0.5Vのオフセットがありましたから、0.5+1.8=2.3と計算通りです。
上下の波形もきちんと出ているようです。
ADの読み取り値も安定しているようなので、何とかなりそうです。一安心です。

さて、次はジャイロコンパスのソフトを試作してロボットを制御してみましょう。

ジャイロセンサの実用回路を考える

前回の実験結果をもとに、実用回路を考えてみます。

単一電源のオペアンプで0V近傍の信号を増幅するには、基本的に信号をオフセットするか、オペアンプを正負電源で動作させるのが定石です。
また、AD変換機の分解能が4.9mVと比較的高感度なので、そこそこ安定度のある回路でなければなりません。無信号時に信号が10mVもふらつくようではダメです。
これらの条件で回路を検討しましょう。

　
案１・Vrefの電圧を変えて直流分を信号に重畳させる

回路図はこのようなものです。センサのVref(基準電圧)を抵抗RAとRBで分圧しVrefより0,25V低いオペアンプ用の基準電圧を作ります。アンプのゲインは約１０倍だから、これで無信号時に0.25X10=2.5Vの出力になりそうです。試してないのであくまでも推測ですが。
電源電圧5Vの中点なのでこれならアンプを有効に使えそうです。
しかし、これには問題があります。分圧回路を入れるとなると、Vrefから多少なりとも電流を引き出さなければなりませんが・・・ジャイロセンサのデータシートにはVrefに関するそういったデータがありません。500uA引き出せるのでしょうか、それとも5mA？そのときの安定度はどれくらいなんでしょう？
試してみるにせよ、安定度の測定となると、ゲイン１０倍のアンプの入力部の安定度ですから、uVくらいの測定が出来る測定器が必要です。テスターとオシロではちょっとばかり心もとないです。

　
案２・ジャイロセンサのGNDを持ち上げる

この回路では、定電圧電源を追加してジャイロセンサのGNDを2Vほど持ち上げてしまいます。これで無信号時の電圧を2Vほどに持っていくことが出来ます。2.5Vにしないのはジャイロセンサの動作電圧を3Vにしたいからです。データシートによると、ジャイロセンサの電源電圧は最低2.7V必要です。
この回路は定電圧回路が必要ですが、この電圧はアンプのゲインに絡まないので、温度変化などによる電圧変動が5mVくらいまでは許容できます。これなら普通の三端子レギュレータでも大丈夫そうです。
部品は増えますが、今後の調整などを考えるとこちらの方が良さそうです。

　
と、いうことで案２のコンセプトで実用回路を試してみます。

ジャイロセンサを試してみる

まず、ジャイロセンサのキットを秋月で買ってきました。完成品モジュールの方が安いですが、チップ部品なので実験には不向きです。説明書通りに組み立てて写真のように実験用ロボに取り付けました。
今回は左右への旋回時の角度を計測する目的なので、念のため、超信地旋回での旋回中心に近い場所を狙いました。

村田のジャイロセンサのデータシートは大変あっさりしたものです。メーカーがこのようなデータしか公開していないということは、おそらく使用に際してのノウハウが多く、顧客の要望に応じて個別に技術対応しているのではないかと思います。
アマチュアはアマチュアらしくお座敷実験でノウハウをつかむのが良さそうです。

データシートにはこんな参考回路が載っています。

秋月のキットも基本的には同じです。ネット上でたくさんの方が議論している通り、入力に微分回路が入っているので、検出できるのは角加速度のように思われます。
ともかく計測してみたいと思います。

計測方法は、ロボットを１秒ほど超信地旋回で旋回させ、そのときのジャイロセンサモジュールの出力（オペアンプの出力）をオシロで確認します。
　
　

■微分回路ありの場合
データシートどおりの回路の場合、波形はこんな風になります。時間軸は500ms/div 電圧軸は500mV/divです。

見たところ、角加速度らしいプロットです。下側のチャンネルBはモーターの駆動期間を示しています。Hレベルのときにモーターを駆動しています。
駆動時間のほとんどは、ほぼ等角加速度運動をし、モーター駆動停止近くになると角加速度が小さくなっているのがわかると思います。その後はブレーキによる急速な角加速度が発生し、おそらく下側のピーク（モーターonから1400msほど）で車体が停止しているのだと思います。
問題は、その後速やかに無信号時の電圧に戻らないことです。微分回路の時定数が大きいのでやむを得ません。
これだと、手ぶれ補正やラジコンヘリのテール制御などには使えますが、「ジャイロコンパス」は無理でしょう。角速度を直接読む必要があります。
　
　

■ダイレクト接続の場合
微分回路の4.7uFのコンデンサをショートし、ジャイロセンサのVoutを直接アンプに入れます。
このときの波形はこんな感じです。

こんどは角速度を表しているのがわかると思います。前の測定結果と突き合わせると、よく適合しているのがわかります。駆動開始近くはほぼ等角加速度運動、終了近くになると角加速度が小さくなってます。モーター駆動停止と同時に角速度は直線的に低下し、開始から1400msほどでゼロに戻ります。

問題は、無信号時の電圧が0.5V程度と低いことです。これは当たり前です。センサに電圧オフセットをかけていないからです。
オペアンプの出力のスイング電圧は、レールツーレールのような特殊なものを使わない限り、よくても電源電圧の８割ぐらい（この回路は5Vだからスイングは0.5Vから4.5Vくらい）ですから、下側の角速度計測値は当てになりません。
この辺をなんとかした回路を設計することにします。

PICでPSDの測定結果を距離に変換

PSDはレンジファインダーの原理で対象物までの距離を測るセンサです。測定結果は電圧で出力されるので、距離に変換する関数を作っておくと便利です。

距離と電圧の関係はこのグラフのようになっています。

これは1/xのグラフを当てはめれば良さそうですので、次の式を使います。

距離 = a/(電圧+b)

aとbは次のように求めます
　１・b=0とおいて式を変形→　a=距離・電圧
　２・上の式で10cmでのaを求める（電圧はグラフから読む）
　３・求めたaを最初の式に代入して60cmでの電圧から距離を求める
　４・求めた距離が60cmに近づくようにbの値を調整する
bを決めると10cmでの電圧と距離にも多少誤差がでてくるので、10cmでの電圧値と60cmの電圧値を交互に入れながらbの値を調整します。この時、必要ならaの値も微調整します。いわゆるトラッキング調整です。中身は違いますが、５球スーパーなんかでやってる周波数調整に感覚は似ています。
この作業はエクセルなどに式を登録して行うと簡単です。

求めたa,bを使った距離の計算式をプログラムしたものがこれです。整数化したので16シリーズのPICでも高速で変換できます。
引数は10bit、AD電圧5.0Vで変換したPSDの電圧、変換結果はmmでの対象物までの距離です。電圧が400mV以下の場合は、反応なしとして0xffffを返します。

//PSD GP2D12の出力電圧をmmに変換(100-800mm)
//longは16bit変数　long longは32bit変数
unsigned long adj_GP2D12(unsigned long sensor_val){
　　unsigned long long xd;
　　//ADは基準電圧5v、10ビットで量子化
　　if(sensor_val < 80) return(0xffff);　//遠すぎる場合(400mvより小さい）0xffffを返す
　　xd = sensor_val * 5;　　　　　　　　//AD変換値をmvに調整（実際は4.9）
　　return(220000 / (xd - 200));　　　//距離調整はこの計算式で
}

220000がa、-200がbです。これは実際のセンサで校正した値です。ケント紙を対象物とし、これの距離を変えて測定距離が正確になるようaとbを微調整します。

これで結果がすぐにmmで読める訳ですが・・・使う上には注意が必要です。
ご存知の方も多いと思いますが、PSDは、形の複雑なもの、平面でも模様があるものなどを計測すると結果がかなりばらつきます。また、理想的な反射体でも、検出物との距離が大きくなるほど値が不安定になります。距離が大きくなるとグラフの傾きが小さくなっていくので、わずかな電圧の変化が、大きな距離の変化になって現れるからです。このことは、小さな電気的ノイズなどが大きな誤差になることを意味していますね。
何回かの計測値の平均を取るなどの前処理をするとよくはなりますが、１回の計測に50msくらいかかるようなので、確定値を得るために時間がかかります。
この辺を理解した上で、この関数を使う必要があるでしょう。例えば、500mmと510mmを見分けなければならないようなプログラムは思ったように動きません。

C#でPSDセンサを「見える化」する

実験用ロボの「たくさんセンサ」は３つのPSDが一組になっています。右側の写真のようにそれぞれのPSDは下向け、水平、上向けにセットしユニット化しました。下向けのセンサは床面を感知し、路面の段差の検出をもくろんでいます。このようなユニットを３つ、ロボットの前部に左側の写真のようにほぼ同じ方向を向けて取り付けました。
このようにセンサを立体的に配置することで、センサによる「認識力」が向上するかどうかを実験してみたいと思います。

　

下向けのセンサは近距離用のGP2D120、他のセンサはGP2D12(GP2Y0A21YK)を使っています。

C＃で作成したコンソールソフトは各PSDの情報を数値で表示するだけでなく、左上のウィンドウにそれぞれのPSDからの距離情報を９個の四角形の明るさで表現するようになっています。反応がなければ黒、最も近い反応で白になります。これがPSDの「見える化」です。ロボットが外界をどんな風に見ているのかが直感的に分かるので、自律プログラムを作る際のツールとして有効だと思います。

いくつか典型的なケースをみてみましょう。

　
背の低い障害物はこんな風に見えます。平らな床では下向きのセンサは常に一定の値を示しています。

　
壁などの背の高い障害物はこんな風です。

　
雑誌のような低い障害物は下向けのセンサで対応します。左下のセンサが他と比べて白っぽくなっています。

　
これは段差のイメージです。左下のセンサが凹んでいる段差を検出しています。

　
テーブルのように、ロボットが潜り込めるものはこんな風に見えています。

静止画だとピンとこないと思いますが、リアルタイムにセンサのイメージを見ながらロボットを操縦するのは、それほど難しくありません。
ちなみにコンソール右下のボタンをクリックして実験用ロボをリモコンすることが出来ます。