　ではもう少し機能を付け加えることにする。猪突猛進だけでは能が無いので、超音波センサーを使って目標までの距離を測定、ある程度以上の距離ならば全力走行だが、近づくとスピードを落とすことにする。また、CMOSセンサーからは単に左右だけでなく、中心からのオフセットの値も取れるようにする。そうすればオフセットと距離から角度が計算できるので、これに応じてステアリングの切り方も変えるようにしよう。

　ついでに、ステアリングを大きく切る場合は、それに応じて速度も落としてやる機能も追加しよう。全体としては図2の様な構成だ。このあたりまでやれば、後はパラメータの調整でだいぶ賢く走るようになる。

図2　超音波センサーを追加、対象までの距離を測定して動きに反映することにした

　問題はこの場合の制御ループだが、下のように最初に挙げたものよりだいぶ処理が増えることになる。

while()
{
    direction = input_from_camera();		// カメラからオフセットを取得
    distance = input_from_sonicsensor();	// 超音波センサーから距離を取得
    angle = getangle( direction, distance );	// 目標の方位角を計算
    steeringoffset = getoffset( angle );	// 方位角からステアリング量を計算
    speed = getspeed( distance, angle );	// 方位角と距離から速度を計算
    output_motor1( steeringoffset );		// ステアリング設定
    output_motor2( speed );			// 速度を設定
}

　角度の算出にあたり、三角関数をまじめに使って計算したら時間がかかって仕方が無いので、あらかじめ距離とオフセットに応じて大まかに角度を出すテーブルを構築しておき、これを参照といった形で実装することになると思う。ただこのテーブルの持ち方を相当工夫しないとメモリが足りなくなる。現実的にはプロセッサーとしてメモリ搭載量の多い32bit MCUを選び、無理やりテーブルを載せるとかの力技に走ってしまう事が多いだろう。I/Oもそうで、2種類のモーターを駆動するのに、1つのプロセッサではちょっと難しい。

　今回は駆動側をDCブラシモータと仮定して辛うじて逃げている（PWM出力1つだけで調整できるからだ）が、これは結構I/Oの種類が多いMCUでないと難しい。またカメラと超音波センサーの入力、2種類のモーターへの制御出力とだいぶI/Oに要する時間が増えてきたが、それでもこのレベルならまだ辛うじてシングルループで廻せる範囲だ。