カテゴリー別アーカイブ: ハードウェア

加速度センサーで距離を推定する

ハウスローバーにとって、走行距離の計測は重要な項目です。 最も基本的な方法は、車輪の回転を検知することですが、車輪のスリップや障害物の影響などを受ける方法でもあります。 今回は、加速度センサーを使用し、その測定値の信頼性を検証します。

MPU-9150 9軸センサーモジュール

MPU-9150 9軸センサーモジュール

MPU-9150は、加速度センサー、ジャイロセンサーと電子コンパスまで付いた、お得感たっぷりのモジュールです。 加速度センサーによる距離測定がものになれば、ジャイロセンサーも統合できますので、何かと便利です。

実験用のプログラムは、ジャイロセンサーの角速度から角度を算出したものを応用し下記の通りとしました。

事前にセンサー値を複数回読み取り補正値を算出 → 計測開始 → 加速度を算出 → 加速度を積算し速度を算出 → 速度を積算し距離を算出  という流れです。

#include <stdio.h>
#include <wiringPiI2C.h>

#define MOTOR1 22           // GPIO22
#define MOTOR2 23           // GPIO23
#define MOTOR3 24           // GPIO24
#define MOTOR4 25           // GPIO25

int main(int argc, char **argv)
{
	int i2c_fd;       		// デバイスファイル用ファイルディスクリプタ
	int i2cAddress = 0x68;	// MPU9150のI2Cアドレス
	int i,regH,regL,out_x,adj,backup = 0,inp,dis = 0;
	float data,acc,spd;

	// I2Cデバイスファイルをオープン
	i2c_fd = wiringPiI2CSetup(i2cAddress);

	// MPU9150 イニシャライズ
	if((wiringPiI2CWriteReg8(i2c_fd,0x6b,0x00))<0){
		printf("write error register 0x6b");
	}
	printf("write register:0x6b0 = 0x00\n");

	//引数の取得
	inp = atoi(argv[1]);

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

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

	// 補正値を計測
	for(i=0; i<20; i++){
		// デバイスからデータ取得
		// read L reg
		regL = wiringPiI2CReadReg8(i2c_fd,0x3c);
		// read H reg
		regH = wiringPiI2CReadReg8(i2c_fd,0x3b);
		out_x = (regH<<24|regL<<16)>>16;

		// 取得したデータを表示
		//printf("i : %d  out_x : %d\n",i,out_x);
		adj = adj + out_x;
		usleep(10000);		//10ms
	}
	adj = adj / 20;			// 平均化し補正値とする
	printf("補正値 : %d\n",adj);
	sleep(1);

	// 前進
	digitalWrite(MOTOR2, 1);
	digitalWrite(MOTOR4, 1);

	// 加速度データを取得、表示
	for(i=0; i<10000; i++){
		// デバイスからデータ取得
		// read L reg
		regL = wiringPiI2CReadReg8(i2c_fd,0x3c);
		// read H reg
		regH = wiringPiI2CReadReg8(i2c_fd,0x3b);
		out_x = (regH<<24|regL<<16)>>16;

		// 取得したデータを表示
		//printf("i : %d  out_x : %d",i,out_x);

		// データを補正し加速度算出
		acc = out_x - adj;
		data = (acc / 16384) * 9.80665;	//加速度を算出
		spd += data * 0.5;				//速度を算出
		if(spd < 1){spd = 0;}
		dis += spd;						//距離を算出

		//printf("  加速度 : %.5f  速度 : %.3f  距離 : %d\n",data,spd,dis);

		//距離をチェックし、指定値に達していたら停止
		if (dis >= inp ){
			//ブレーキ
			digitalWrite(MOTOR1, 1);
			digitalWrite(MOTOR4, 1);
			digitalWrite(MOTOR2, 1);
			digitalWrite(MOTOR3, 1);
			usleep(100000);
			//停止
			digitalWrite(MOTOR1, 0);
			digitalWrite(MOTOR4, 0);
			digitalWrite(MOTOR2, 0);
			digitalWrite(MOTOR3, 0);
			return;
		}
		usleep(10000);		//10ms
	}

	return;
}

30センチの距離を数十回テストしましたが、全く安定しません。 毎回、2~3センチのズレが生じ、当然のことながら距離が長くなれば、誤差も大きくなっていきます。

パラメータや算出方法を変えて行ってみましたが、信頼できる結果は得られませんでした。 残念です。

フォトリフレクタを使用した、車輪の回転数による距離計測を使用すべきだと実感した実験結果となりました。

 

 

Raspberry Pi 2 の誘惑

省電力を必要としているハウスローバーでは、本来、modelA+やB+への切り替えが良いのでしょうが、高性能への誘惑を断ち切れず、RasPi2にしてしまいました。 (Amazonの「当日お急ぎ便カウントダウンタイマー」も衝動に拍車をかけましたが…)

RasPi2

RasPi2と40P-26Pケーブル

40P-26Pダウングレードケーブルも同時購入しましたので、なんの変更もなく差し替えるだけです。

また、ソフトウェアも最新のRaspbianを使用しているので、そのまま動くはず…でした。

ところが、起動はするものの、パン・チルトが動作しません。 ServoBlasterに問題があるようです。

よくよく調べてみるとフォーラムに情報がありました。

http://www.raspberrypi.org/forums/viewtopic.php?f=28&t=99115&start=25

一部のメモリマッピングに変更があるようです。 上記ページの最下部に修正ファイルがアップされています。 (ServoBlaster-20150219.tgz)

このファイルを適当なフォルダへ解凍しmake installを実行。 無事、ServoBrasterも従来通り動作するようになりました。

さて、RasPi2の使用感ですが、ストリーミングの遅延が減少し、操作時の反応が早くなった? ような気がします。 (^^ゞ

気になる消費電力ですが、modelBからの切り替えですので確実に改善しています。 モジュール単体の実測値では、modelB:370mA  Pi2:260mA との結果です。(低負荷時)

topコマンドで確認すると、4つのコアがそこそこ使用されていることが確認できます。

top

現状では、これといって大きく改善したというほどではありませんが、処理速度の問題で見送っていた項目、例えばmotionによる画像認識処理などの選択の幅が広がったという面では、意味のある切り替えだと思います。

 

リアルタイムクロック RTC-8564NB で起動する

ESAの彗星探査機「フィラエ」が電力喪失、スリープモードに移行

探査機にとって電力は命です。 フィラエは太陽光パネルに光が当たれば、復活の可能性がありますが、電力源としてバッテリーしか持たないハウスローバーは、それを消費してしまえば、活動できなくなります。 少しでも長くバッテリーを持続させるための制御に利用する計画で、RTCを搭載しました。

RTC-8564NB

RTC-8564NBは、分、時、日、曜日を指定できるアラームと0~255秒(または分)を設定できるタイマーを内蔵しています。 その出力としてINT端子がLowレベルとなるため、Raspberry Piのリセット端子と接続することで、シャットダウン状態のシステムを起動することができます。

RTC_INT配線

RTCのINT端子部分の配線

 

P6の1番ピンへ接続

P6の1番ピンへ接続

I2C接続後、ハードウェアクロックデバイスとして登録します。

/etc/modules-load.d/rasberrypi.conf へ
rtc-pcf8563 を、追加
echo pcf8563 0x51 > /sys/class/i2c-adapter/i2c-1/new_device を、実行する。

これで、hwclockコマンドが使用できるようになります。

ところが、システムモジュールとして登録すると、I2Cコマンドで直接操作ができなくなります。 (pythonでは可能なようですが…)

今回は、アラーム設定を直接操作したいので、システムモジュールには登録しません。 (システムモジュールとして登録されていて、一時的に外す場合は modprobe -r rtc-pcf8563 を実行)

PHPで、RTCを設定するスクリプトを作成しました。

<?php
//RTC SET v1.1 2015.1.11 spacelab
//ハードウェアクロックとして設定されている場合は
//タイムゾーンはUTCが使用されています。
//本スクリプトはJSTとして動作しますので、事前に
//時刻設定を行って下さい。

date_default_timezone_set('Asia/Tokyo');
setlocale(LC_ALL,"ja_JP.UTF-8");

$i2cadd = 0x51; //デバイススレーブアドレス

$msk3 = 0x07;
$msk5 = 0x1f;
$msk7 = 0x3f;
$msk8 = 0x7f;
$bit2 = 0x02;

if($argv[1]){ //パラメータチェック
 $sw = $argv[1];
}else{
 $sw = "-d";
}

switch ($sw){
 case "-d":
 //RTC時刻の取得
 //レジスタの読み込み
 $r_sec = hexdec(exec("i2cget -y 1 $i2cadd 0x02"));
 $r_min = hexdec(exec("i2cget -y 1 $i2cadd 0x03"));
 $r_hou = hexdec(exec("i2cget -y 1 $i2cadd 0x04"));
 $r_day = hexdec(exec("i2cget -y 1 $i2cadd 0x05"));
 //$r_wek = exec("i2cget -y 1 0x51 0x06");
 $r_mon = hexdec(exec("i2cget -y 1 $i2cadd 0x07"));
 $r_yea = substr(exec("i2cget -y 1 $i2cadd 0x08"),2) + 2000;

 //不要ビットをマスク
 $r_sec = dechex($r_sec & $msk8);
 $r_min = dechex($r_min & $msk8);
 $r_hou = dechex($r_hou & $msk7);
 $r_day = dechex($r_day & $msk7);
 $r_mon = dechex($r_mon & $msk5);

 $timestamp = mktime($r_hou,$r_min,$r_sec,$r_mon,$r_day,$r_yea,0);
 echo strftime("%c\n",$timestamp);
 break;

 case "-s":
 //RTC時刻をシステムタイマーと同期
 $r_yea = date('y');
 $r_mon = date('m');
 $r_day = date('d');
 $r_wek = date('w');
 $r_hou = date('H');
 $r_min = date('i');
 $r_sec = date('s');

 exec("i2cset -y 1 $i2cadd 0x08 0x".$r_yea);
 exec("i2cset -y 1 $i2cadd 0x07 0x".$r_mon);
 exec("i2cset -y 1 $i2cadd 0x06 0x".$r_wek);
 exec("i2cset -y 1 $i2cadd 0x05 0x".$r_day);
 exec("i2cset -y 1 $i2cadd 0x04 0x".$r_hou);
 exec("i2cset -y 1 $i2cadd 0x03 0x".$r_min);
 exec("i2cset -y 1 $i2cadd 0x02 0x".$r_sec);

 $timestamp = mktime($r_hou,$r_min,$r_sec,$r_mon,$r_day,$r_yea,0);
 echo strftime("%c\n",$timestamp);
 break;

 case "-w":
 //RTCをシステム時刻に設定
 //レジスタの読み込み
 $r_sec = hexdec(exec("i2cget -y 1 $i2cadd 0x02"));
 $r_min = hexdec(exec("i2cget -y 1 $i2cadd 0x03"));
 $r_hou = hexdec(exec("i2cget -y 1 $i2cadd 0x04"));
 $r_day = hexdec(exec("i2cget -y 1 $i2cadd 0x05"));
 //$r_wek = exec("i2cget -y 1 0x51 0x06");
 $r_mon = hexdec(exec("i2cget -y 1 $i2cadd 0x07"));
 $r_yea = substr(exec("i2cget -y 1 $i2cadd 0x08"),2) + 2000;

 //不要ビットをマスク
 $r_sec = dechex($r_sec & $msk8);
 $r_min = dechex($r_min & $msk8);
 $r_hou = dechex($r_hou & $msk7);
 $r_day = dechex($r_day & $msk7);
 $r_mon = dechex($r_mon & $msk5);

 $timestr = "$r_yea/$r_mon/$r_day $r_hou:$r_min:$r_sec";
 exec("date --set='".$timestr."'");
 echo exec("date")."\n";
 break;

 case "-as":
 //アラーム時刻の設定
 if($argv[2]){
 if($argv[2] =="m"){$a_cas = "0x09";}
 if($argv[2] =="h"){$a_cas = "0x0a";}
 if($argv[2] =="d"){$a_cas = "0x0b";}
 if($argv[2] =="w"){$a_cas = "0x0c";}
 if($argv[3]){
 $a_val = hexdec($argv[3]);
 }else{
 $a_val = 128;
 }
 exec("i2cset -y 1 $i2cadd $a_cas $a_val");
 exec("i2cset -y 1 $i2cadd 0x01 0x02");

 }else{
 //アラームセット
 exec("i2cset -y 1 $i2cadd 0x01 0x02");
 echo "アラームセットしました\n";
 }
 case "-a":
 //アラーム時刻の表示
 $a_set = hexdec(exec("i2cget -y 1 $i2cadd 0x01"));
 $a_min = hexdec(exec("i2cget -y 1 $i2cadd 0x09"));
 $a_hou = hexdec(exec("i2cget -y 1 $i2cadd 0x0a"));
 $a_day = hexdec(exec("i2cget -y 1 $i2cadd 0x0b"));
 $a_wek = hexdec(exec("i2cget -y 1 $i2cadd 0x0c"));

 if ($a_min >=128){$a_min_on = "無効 ";}else{$a_min_on = "有効 ";}
 if ($a_hou >=128){$a_hou_on = "無効 ";}else{$a_hou_on = "有効 ";}
 if ($a_day >=128){$a_day_on = "無効 ";}else{$a_day_on = "有効 ";}
 if ($a_wek >=128){$a_wek_on = "無効 ";}else{$a_wek_on = "有効 ";}

 $a_min = dechex($a_min & $msk8);
 $a_hou = dechex($a_hou & $msk7);
 $a_day = dechex($a_day & $msk7);
 $a_wek = dechex($a_wek & $msk3);

 echo "アラーム設定";
 if (dechex($a_set & $bit2) == 2){echo " 有効\n";}else{echo " 無効\n";}
 echo "Minuite: $a_min_on $a_min\n";
 echo "Hour : $a_hou_on $a_hou\n";
 echo "Day : $a_day_on $a_day\n";
 echo "Week : $a_wek_on $a_wek\n";
 break;

 case "-ar":
 //アラームリセット
 exec("i2cset -y 1 $i2cadd 0x01 0x00");
 echo "アラームリセットしました\n";
 break;

 case "-ts":
 //タイマーの設定
 if($argv[2]){
 exec("i2cset -y 1 $i2cadd 0x0f $argv[2]");
 if($argv[3]){
 exec("i2cset -y 1 $i2cadd 0x0e 0x83");
 }else{
 exec("i2cset -y 1 $i2cadd 0x0e 0x82");
 }
 exec("i2cset -y 1 $i2cadd 0x01 0x01");
 }else{
 echo "パラメータが足りません\n";
 }

 case "-t":
 //タイマー情報の表示
 $t_cnt = hexdec(exec("i2cget -y 1 $i2cadd 0x0e"));
 $t_dat = hexdec(exec("i2cget -y 1 $i2cadd 0x0f"));

 echo "タイマー設定";
 if ($t_cnt >=128){echo " 有効\n";}else{echo " 無効\n";}
 if ($t_cnt & 1){$clock = "分";}else{$clock = "秒";}
 echo "Timer: $t_dat $clock\n";
 break;

 case "-tr":
 //タイマーリセット
 exec("i2cset -y 1 $i2cadd 0x01 0x00");
 exec("i2cset -y 1 $i2cadd 0x0e 0x00");
 echo "タイマーリセットしました\n";
 break;

 case "-h":
 case "-?":
print <<< EOS
使用法: rtcset.php [機能] [設定値]...
機能:
 -d 現在時刻を表示する
 -s RTCをシステムクロックと同期する
 -w RTCの時刻をシステムクロックへ設定する
 -a アラームの設定状況を表示する
 -as [m,h,d,w] [値]
 アラームを有効に設定する
   [m,h,d,w] 分,時,日,曜日の項目を指定する
 [値] を省略すると該当項目を無効にする
 曜日の値:日=0 月=1 火=2 水=3 木=4 金=5 土=6
 例: 10時30分に設定
 -as h 10
 -as m 30
 -ar アラームをリセットし、無効に設定する
 -t タイマーの設定状況を表示する
 -ts [値] [秒,分]
 タイマーを有効に設定する
 [値] 0-255(秒) 単位: [初期値=秒,1=分]
 -tr タイマーをリセットし、無効に設定する
 -h,-? 使い方を表示
 -v バージョン情報を表示

EOS;
 break;

 case "-v":
 echo "rtcset.php version 1.1\nCopyright (C) 2015 space laboratory\n";
 break;

}
?>

php rtcset.php -h で、ヘルプが表示されます。 アラームやタイマーの設定をして、シャットダウンを実行することで、指定の時刻に起動させる事ができるようになりました。

アラーム出力はRaspberry Piにリセットをかけるものですので、メインのソフトウェアで定期的にタイマーを更新するような処理を行うことで、システム異常停止の際に復帰させる、ウォッチドッグタイマーの様な使い方もできると思います。