前回の記事でMPU6050のセンサーの補正を行いました。
リアルタイム描画
今回はこちらの記事を参考にさせていただきました。(matplotlibでリアルタイム描画)
ソースコードは別の章で記載します。
図1にセンサーを適当に動かした結果を示します。
 |
| 図1:プロット結果 |
定性的な評価になってしまいますが、1秒程度のラグがあるように感じました。
ラズパイZeroで実行しており、処理が追いついてないこともしくはリモートデスクトップで接続していることによる通信ラグがあるかと思います。。。
ラズパイ４を買ったため準備が出来次第マシンパワーを上げればスムーズに描画できるかを確認したいと思います。
基準の円を描いたうえでセンサーからの取得値を描画する。
今回は車両で使うことを想定しているため、摩擦円を意識した荷重移動の練習が出来るように基準の円を描いたうえでMPU6050からのデータを描画していきます。
今回は基準として0.2 G 0.5G 1G 1.5Gの円を描いていきます。
円の描画についてはこちらの記事を参照しました。
ちょっと苦戦したところは円を描画した後にadd_artistを使わないと描画が出来なかったというところ程度ですかね？
詳しくは下記2つの記事を参照してください。
描画した結果を図2に示します。
 |
| 図2：基準円を追加した場合の描画結果 |
まともに使うことが出来ない状況だったため、ラズパイZeroではリアルタイム描画はあきらめた方がいいかもしれないです。。。
ソースコード
今回使用したソースコードを示します。
例のごとく動くことを重視しているため、整理はできてないです。。。
あくまで参考レベルです。。。
import smbus
import math
from time import sleep
import time
import sys
import traceback
import matplotlib.pyplot as plt
import datetime
#各種レジスタのアドレス
#----------------------------------------------------------------------------------
DEV_ADDR = 0x68
CONFIG = 0x1A
GYRO_CONFIG = 0x1B
ACCEL_CONFIG = 0x1C
ACCEL_XOUT = 0x3b
ACCEL_YOUT = 0x3d
ACCEL_ZOUT = 0x3f
TEMP_OUT = 0x41
GYRO_XOUT = 0x43
GYRO_YOUT = 0x45
GYRO_ZOUT = 0x47
PWR_MGMT_1 = 0x6b
PWR_MGMT_2 = 0x6c
#----------------------------------------------------------------------------------
#定数
#----------------------------------------------------------------------------------
CAL_SIZE = 5000#キャリブレーションに使うデータ数
IGNORE_DATA_SIZE = 50#キャリブレーションを行う際に最初の方のデータは信頼性が無いため、何個のデータを無視するか設定
X_BUF_SIZE = 5 #描画する際に何個の点をプロットするか
Y_BUF_SIZE = 5 #描画する際に何個の点をプロットするか
Z_BUF_SIZE = 5 #描画する際に何個の点をプロットするか
PLLOT_TIME = 0.01#何秒間隔でプロットするか
X_OFFSET = -703#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は-703を入れる)
Y_OFFSET = 394#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は394を入れる)
Z_OFFSET = 1867#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は1867を入れる)
#----------------------------------------------------------------------------------
#変数/配列の初期化処理
#----------------------------------------------------------------------------------
data_x_list = []#データ格納用の配列作成
data_y_list = []
data_z_list = []
data_x_offset_list = []#データ格納用の配列作成(オフセット後)
data_y_offset_list = []
data_z_offset_list = []
bus = smbus.SMBus(1)
#リアルタイム描画する際に必要な処理
#----------------------------------------------------------------------------------
fig,axes = plt.subplots(1,1)
lines, = axes.plot(0,0,"-o",lw=5)#何かプロットしないといけないとのことなので、空プロット
plt.grid()
axes.set_xlabel("X_AXIS[G]")
axes.set_ylabel("Y_AXIS[G]")
buf_x =[]
buf_y =[]
buf_z =[]
#----------------------------------------------------------------------------------
# x_offset = -703#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は-703を入れる)
# y_offset = 394#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は394を入れる)
# z_offset = 1867#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は1867を入れる)
#----------------------------------------------------------------------------------
#デバイスの初期化処理
#----------------------------------------------------------------------------------
# bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0x80)#write_byte_data(int addr,char cmd,char val) 1:I2C通信対象のアドレス。今回は加速度センサのみなので68固定,2:レジスタのアドレス,3:値)0x80はデバイスリセット
bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0x00)
# bus.write_byte_data(DEV_ADDR, PWR_MGMT_2, 0x07)#FIFO/I2C_MST/SIG_CONDのリセット
bus.write_byte_data(DEV_ADDR, PWR_MGMT_2, 0x00)
#----------------------------------------------------------------------------------
#センサーの設定
#----------------------------------------------------------------------------------
# bus.write_byte_data(DEV_ADDR, CONFIG, 0x00)#CONFIG(フィルターなし)
bus.write_byte_data(DEV_ADDR, CONFIG, 0x06)#CONFIG(フィルター最大)
bus.write_byte_data(DEV_ADDR, GYRO_CONFIG, 0x00)#250°/s
bus.write_byte_data(DEV_ADDR, ACCEL_CONFIG, 0x00)#2g
#----------------------------------------------------------------------------------
def read_word(adr):#センサーの値が2バイトであらわされるため、2バイト分のデータを結合する関数
high = bus.read_byte_data(DEV_ADDR, adr)#上位バイトのデータ
low = bus.read_byte_data(DEV_ADDR, adr+1)#下位バイトのデータ
val = (high << 8) + low#上位バイトのデータをビット1バイト分(8bit)左にシフトして、下位バイトのデータと足し合わせて1つのデータにしている。
return val
def read_word_sensor(adr):
val = read_word(adr)
if (val >= 0x8000): return -((65535 - val) + 1)#0x8000が最上位ビットが立っている状態なので、マイナス。もし、マイナス値だった場合は符号を反転させてる。(ただ反転させるだけではダメなので注意)
else: return val
def get_temp():
temp = read_word_sensor(TEMP_OUT)
x = temp / 340 + 36.53 # data sheet(register map)記載の計算式.
return x
def getGyro():
x = read_word_sensor(GYRO_XOUT)/ 131.0# data sheet(register map)記載の計算式.(250°を選択した場合)
y = read_word_sensor(GYRO_YOUT)/ 131.0
z = read_word_sensor(GYRO_ZOUT)/ 131.0
return [x, y, z]
def getAccel():
x = read_word_sensor(ACCEL_XOUT)/ 16384.0# data sheet(register map)記載の計算式.(2gを選択した場合)
y = read_word_sensor(ACCEL_YOUT)/ 16384.0
z = read_word_sensor(ACCEL_ZOUT)/ 16384.0
return [x, y, z]
def getAccel_offset(): #オフセット分の考慮した場合のデータ
x = (read_word_sensor(ACCEL_XOUT)+X_OFFSET)/ 16384.0# data sheet(register map)記載の計算式.(2gを選択した場合)
y = (read_word_sensor(ACCEL_YOUT)+Y_OFFSET)/ 16384.0
z = (read_word_sensor(ACCEL_ZOUT)+Z_OFFSET)/ 16384.0
return [x, y, z]
def real_time_plot():
global lines
global axes
global buf_x
global buf_y
global buf_z
while True:
x,y,z = getAccel_offset()
buf_x.append(x)
buf_y.append(y)
buf_z.append(z)
#何個の点を描画するか
if len(buf_x) > X_BUF_SIZE:
del buf_x[0]
if len(buf_y) > Y_BUF_SIZE:
del buf_y[0]
if len(buf_z) > Z_BUF_SIZE:
del buf_z[0]
draw_circle = plt.Circle(xy=(0, 0), radius=0.2,fill=False,ec = "b")
axes.add_artist(draw_circle)
draw_circle2 = plt.Circle(xy=(0, 0), radius=0.5,fill=False,ec = "b")
axes.add_artist(draw_circle2)
draw_circle3 = plt.Circle(xy=(0, 0), radius=1.0,fill=False,ec = "b")
axes.add_artist(draw_circle3)
draw_circle4 = plt.Circle(xy=(0, 0), radius=1.5,fill=False,ec = "b")
axes.add_artist(draw_circle4)
# plt.Circle(xy=(0, 0), radius=1.5,fill=False,ec = "b")
lines.set_data(buf_x,buf_y)
print("bus_x,bus_y")
print(buf_x)
print(buf_y)
axes.set_xlim(-2.1,2.1)#今回は2gのセンサーを使っている為、範囲は2g+マージンで0.1の範囲で描画
axes.set_ylim(-2.1,2.1)#今回は2gのセンサーを使っている為、範囲は2g+マージンで0.1の範囲で描画
plt.pause(PLLOT_TIME)
print("lecoding")
real_time_plot()
まとめ
今回は加速度センサーの値をリアルタイムでプロットする処理を追加しました。
結論から言うとラズパイZeroではリアルタイムでプロットするのは難しいことが分かりました。
ラズパイ4で試してみるか、センサー値をノートPC側で受信したうえで描画をノートPCでやる方法かなと思います。まぁ実際は走りながら画面を見る余裕はない気がするため、あとからPCでデータ解析する際にとしてこの処理を活用しようと思います。
.png)
0 件のコメント:
コメントを投稿