TPR-105 | SPACE LABORATORY西東京　ロボット研究所

加速度センサーによる距離の推定結果が思わしくなかったため、オーソドックスなフォトインタラプタ（フォトリフレクタ）を使用し、距離を測定することとしました。　使用したのは、TPR-105という反射型の素子です。　透過型の方が扱いが楽なのですが、スペースの問題により、この素子を選択しました。

TPR-105

反射型の場合、対象との距離や反射率、外乱光などの影響を受け易いため、受光部の調整ができるようにすべきなのでしょうが、実験を繰り返し最適値を導きました。

実験の様子

反射板のパターン作成にはVegaさんのロータリ・エンコーダ用パターン作成ソフトを利用させていただきました。　winXP用に作成されたソフトのようですが、win7でも問題なく動作しました。

回路は丸型ユニバーサル基板 (32mm)に作成し、モーターへ両面テープで取付ます。　センサーと反射板のギャップは2～3ミリです。　当初、発光部のダイオードは5Vを供給していましたが、黒パターンの認識が不安定であった為、3.3Vへ変更しました。　また、実機に搭載した際にモーターノイズの影響と思われる誤カウントが見られましたので、出力部分へコンデンサを追加しました。

センサー基板

センサー基板　回路図

※2016.4.3　出力部の抵抗を10KΩへ変更しました。

ソフトウェアは一定時間毎にセンサー値を読み込み、その変化をカウントしていきます。　パルス幅を計測する速度測定などは、処理速度の面で無理がありますので、単純にカウントのみとしています。

//motor_enc1.c v1.0 201505016
#include <stdio.h>

#define MOTOR1 22           // GPIO22
#define MOTOR2 23           // GPIO23
#define MOTOR3 24           // GPIO24
#define MOTOR4 25           // GPIO25
#define LEFT_STR  "l"
#define RIGHT_STR "r"
#define FWRD_STR  "f"
#define BACK_STR  "b"

int main(int argc, char **argv)
{

	int inp,bak,enc=0,tout=0,i=0;

	//引数の取得
	int con = atoi(argv[2]);

	// WiringPi イニシャライズ
	if(wiringPiSetupGpio() == -1) return;

	// モーター停止
	digitalWrite(MOTOR1, 0);
	digitalWrite(MOTOR2, 0);
	digitalWrite(MOTOR3, 0);
	digitalWrite(MOTOR4, 0);

	if ( strcmp( argv[1], FWRD_STR ) == 0 ){
		//前進
		digitalWrite(MOTOR2, 1);
		digitalWrite(MOTOR4, 1);
	} else if ( strcmp( argv[1], BACK_STR ) == 0 ){
		//後退
		digitalWrite(MOTOR1, 1);
		digitalWrite(MOTOR3, 1);
	} else if ( strcmp( argv[1], LEFT_STR ) == 0 ){
		//左ターン
		digitalWrite(MOTOR1, 1);
		digitalWrite(MOTOR4, 1);
	} else if ( strcmp( argv[1], RIGHT_STR ) == 0 ){
		//右ターン
		digitalWrite(MOTOR2, 1);
		digitalWrite(MOTOR3, 1);
	}

	usleep(10000);			//10ms
	bak = digitalRead(27);	//センサー値をバックアップ

	while(1){
		inp = digitalRead(27);	//センサー値を読み込み
		if(inp != bak){			//センサー値が変化しているか
			i++;
			if(i >= 2){			//同一の判定結果が2回連続しているか
				enc++;			//カウントアップ
				bak = inp;		//センサー値をバックアップ
				i = 0;
				tout = 0;
			}
		}else{
			i =0;
			tout++;				//タイムアウト値をカウント
		}
		//カウント数をチェックし、指定値に達していたら停止
		if(enc >= con || tout > 100){
			//ブレーキ
			digitalWrite(MOTOR1, 1);
			digitalWrite(MOTOR4, 1);
			digitalWrite(MOTOR2, 1);
			digitalWrite(MOTOR3, 1);
			usleep(500000);
			//停止
			digitalWrite(MOTOR1, 0);
			digitalWrite(MOTOR4, 0);
			digitalWrite(MOTOR2, 0);
			digitalWrite(MOTOR3, 0);
			return;
		}
		usleep(1500);			//1.5ms
	}
	return;
}

実行は、第1引数に方向を、第2引数にカウント値を指定します。

./motor_enc1 f 24 （24パルス前進）

距離の測定誤差は、1メートルで1センチ程度でした。　停止直前に減速させるなどの処理を行うことにより、バッテリー容量などによる誤差を小さくできると思いますが、現在は行っていません。

今後、自動走行パターンを簡単にプログラムできるインターフェースを作成したいと思います。