RaspberryPi でセンサーを使おう

今回のCansatに搭載するのは

• GPSモジュール PA6C
• デジタルコンパス HMC5883L
• 加速度ジャイロセンサ MPU6050
• 気圧センサ BMP180
• 超音波センサ 

の５つ。

これらを統合して20msec周期での制御ができればよいと考えています。
今回の記事では上３つを扱うPythonプログラムを公開します。
拾ってきたプログラムを改変して、ある一定の時間だけログをとり続けるためのプログラムにしました。
ファイルの読み書きをするプログラムを書くよりも、標準出力するプログラムを

$ ./Program.py > data.txt

のように実行した方が楽です。

GPS PA6C

今回のGPSの取得はRapsberryPi (B+) のGPIOピンにあるTX0,RX0を使ってシリアル通信をします。とりあえず読み込んでファイルに書き込むだけのプログラムです。他のセンサも同様ですが、行頭にプログラム開始からの時間を出力しています。

	#!/usr/bin/python
	import sys
	import serial
	import time
	import datetime
	ser=serial.Serial("/dev/ttyAMA0",9600)
	start = datetime.datetime.now()
	end = start+datetime.timedelta(seconds=60)
	print start
	while True:
	line=ser.readline()
	print "%s,%s"%(datetime.datetime.now()-start,line)
	if end-datetime.datetime.now()<datetime.timedelta(seconds=0):
	sys.exit(0)

view raw GPS.py hosted with ❤ by GitHub

デジタルコンパス HMC5883L

通信はI2Cです。pythonでI2C通信をするためにはインポートしなくてはいけないモジュールがあります。
今後は35行目のスケールを自動補正するプログラムが必要となります。

	#!/usr/bin/python
	import sys
	import smbus
	import time
	import math
	import datetime
	bus = smbus.SMBus(1)
	address = 0x1e
	def read_byte(adr):
	return bus.read_byte_data(address, adr)
	def read_word(adr):
	high = bus.read_byte_data(address, adr)
	low = bus.read_byte_data(address, adr+1)
	val = (high << 8) + low
	return val
	def read_word_2c(adr):
	val = read_word(adr)
	if (val >= 0x8000):
	return -((65535 - val) + 1)
	else:
	return val
	def write_byte(adr, value):
	bus.write_byte_data(address, adr, value)
	write_byte(0, 0b01110000) # Set to 8 samples @ 15Hz
	write_byte(1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
	write_byte(2, 0b00000000) # Continuous sampling
	scale = 0.92
	start = datetime.datetime.now()
	end = start+datetime.timedelta(seconds=60)
	print start
	while True:
	x_out = read_word_2c(3) * scale
	y_out = read_word_2c(7) * scale
	z_out = read_word_2c(5) * scale
	bearing = math.atan2(y_out, x_out)
	if (bearing < 0):
	bearing += 2 * math.pi
	print "%s,%f"%(datetime.datetime.now()-start,math.degrees(bearing))
	if end-datetime.datetime.now()<datetime.timedelta(seconds=0):
	sys.exit(0)

view raw HMC5883L.py hosted with ❤ by GitHub

加速度センサ MPU6050

通信はI2Cです。
加速度は距離を時間に関して２回微分した値なので２回積分しなくてはいけません。
ジャイロは角速度なので１回微分すると角度になります。
積分操作をする場合は何かデータを処理してからではないと、多くの誤差を含んでしまいます。
そのための操作（フィルタリング）について考える必要があります。

	#!/usr/bin/python
	import sys
	import smbus
	import math
	import time
	import datetime
	# Power management registers
	power_mgmt_1 = 0x6b
	power_mgmt_2 = 0x6c
	def read_byte(adr):
	return bus.read_byte_data(address, adr)
	def read_word(adr):
	high = bus.read_byte_data(address, adr)
	low = bus.read_byte_data(address, adr+1)
	val = (high << 8) + low
	return val
	def read_word_2c(adr):
	val = read_word(adr)
	if (val >= 0x8000):
	return -((65535 - val) + 1)
	else:
	return val
	def dist(a,b):
	return math.sqrt((a*a)+(b*b))
	def get_y_rotation(x,y,z):
	radians = math.atan2(x, dist(y,z))
	return -math.degrees(radians)
	def get_x_rotation(x,y,z):
	radians = math.atan2(y, dist(x,z))
	return math.degrees(radians)
	bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
	address = 0x68 # This is the address value read via the i2cdetect command
	# Now wake the 6050 up as it starts in sleep mode
	bus.write_byte_data(address, power_mgmt_1, 0)
	start = datetime.datetime.now()
	end = start+datetime.timedelta(seconds=60)
	print start
	now = datetime.datetime.now()
	while 1:
	#now = datetime.now()
	gyro_xout = read_word_2c(0x43)
	gyro_yout = read_word_2c(0x45)
	gyro_zout = read_word_2c(0x47)
	accel_xout = read_word_2c(0x3b)
	accel_yout = read_word_2c(0x3d)
	accel_zout = read_word_2c(0x3f)
	print "%s,%d,%d,%d,%d,%d,%d"%(datetime.datetime.now()-start,accel_xout,accel_yout,accel_zout,gyro_xout,gyro_yout,gyro_zout)
	#time.sleep(0.5)
	if end-datetime.datetime.now()<datetime.timedelta(seconds=0):
	sys.exit(0)

view raw MPU6050.py hosted with ❤ by GitHub