HAVRM mine / MMA7361

これは MMA7361 3軸アナログ加速度センサ の Nucleo F401RE 用のライブラリです。詳しい説明はLibrary中のReadMe.hに記載しています。文字化けや開けない場合はダウンロードしてみてください。Google Chrome では見れるはずです。

Dependents:   Nucleo_L3GD20_MMA7361_Kalman

ReadMe.h@6:ca882e5e2686, 2016-09-30 (annotated)

Committer:
hirokimineshita
Date:
Fri Sep 30 06:50:24 2016 +0000
Revision:
6:ca882e5e2686
Parent:
2:78a6ee76433c 
+a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirokimineshita 2:78a6ee76433c 1 /*
hirokimineshita 0:ad6f3a862ed5 2 これは MMA7361 3軸アナログ加速度センサ の Nucleo F401RE 用のライブラリです。AnalogInポート によるA/D変換をしています。
hirokimineshita 0:ad6f3a862ed5 3 センサのスリープモードを解除するためにSLピンにVDDを入れてください。
hirokimineshita 0:ad6f3a862ed5 4 0gピンを使うことでセンサが自由落下しているときにhighを返すらしいですが、試してはいません。
hirokimineshita 0:ad6f3a862ed5 5 今まで使ってきて、STピンが働いているのかはよくわかりませんでした。最初コンパイルする際注意が出てくるかもしれませんが、
hirokimineshita 0:ad6f3a862ed5 6 無視してください。
hirokimineshita 0:ad6f3a862ed5 7 参考:http://shokai.org/blog/archives/7701
hirokimineshita 0:ad6f3a862ed5 8
hirokimineshita 0:ad6f3a862ed5 9 初期化(nameはなんでもいいです)
hirokimineshita 0:ad6f3a862ed5 10
hirokimineshita 0:ad6f3a862ed5 11 mma7361 name;
hirokimineshita 0:ad6f3a862ed5 12
hirokimineshita 0:ad6f3a862ed5 13 Xピン:PA_0 Yピン:PA_1 Zピン:PA_4 gSピン:PH_1 STピン:PH_0
hirokimineshita 0:ad6f3a862ed5 14 で接続します。
hirokimineshita 0:ad6f3a862ed5 15
hirokimineshita 0:ad6f3a862ed5 16
hirokimineshita 0:ad6f3a862ed5 17 mma7361 name(X,Y,Z);
hirokimineshita 0:ad6f3a862ed5 18
hirokimineshita 0:ad6f3a862ed5 19 Xピン:X Yピン:Y Zピン:Z gSピン:PH_1 STピン:PH_0
hirokimineshita 0:ad6f3a862ed5 20 で接続します。
hirokimineshita 0:ad6f3a862ed5 21
hirokimineshita 0:ad6f3a862ed5 22
hirokimineshita 0:ad6f3a862ed5 23 mma7361 name(X,Y,Z,ST);
hirokimineshita 0:ad6f3a862ed5 24
hirokimineshita 0:ad6f3a862ed5 25 Xピン:X Yピン:Y Zピン:Z gSピン:PH_1 STピン:ST
hirokimineshita 0:ad6f3a862ed5 26 で接続します。
hirokimineshita 0:ad6f3a862ed5 27
hirokimineshita 0:ad6f3a862ed5 28
hirokimineshita 0:ad6f3a862ed5 29 mma7361 name(X,Y,Z,ST,gS);
hirokimineshita 0:ad6f3a862ed5 30
hirokimineshita 0:ad6f3a862ed5 31 Xピン:X Yピン:Y Zピン:Z gSピン:gS STピン:ST
hirokimineshita 0:ad6f3a862ed5 32 で接続します。
hirokimineshita 0:ad6f3a862ed5 33
hirokimineshita 0:ad6f3a862ed5 34
hirokimineshita 0:ad6f3a862ed5 35 関数
hirokimineshita 0:ad6f3a862ed5 36
hirokimineshita 0:ad6f3a862ed5 37 name.calibration();
hirokimineshita 0:ad6f3a862ed5 38
hirokimineshita 0:ad6f3a862ed5 39 PCでターミナルソフトを用いて使います。Serialの設定は必要ありません。
hirokimineshita 0:ad6f3a862ed5 40 TeraTerm での動作確認済みです。センサの値を取得、
hirokimineshita 0:ad6f3a862ed5 41 初期値等を2次元配列 mat[4][3] (3×4行列) に入れます。
hirokimineshita 0:ad6f3a862ed5 42 ターミナルソフト上に出る指示に従ってやれば問題ないです。
hirokimineshita 0:ad6f3a862ed5 43 配列 mat[4][3] については mma7361.cpp に記載してます。
hirokimineshita 0:ad6f3a862ed5 44
hirokimineshita 0:ad6f3a862ed5 45 name.set_each_num(A00,A10,A20,A01,A11,A21,G00,G10,G20,G01,G11,G21,range);
hirokimineshita 0:ad6f3a862ed5 46
hirokimineshita 0:ad6f3a862ed5 47 | A00 A10 A20 |
hirokimineshita 0:ad6f3a862ed5 48 mat = | A01 A11 A21 |
hirokimineshita 0:ad6f3a862ed5 49 | G00 G10 G20 |
hirokimineshita 0:ad6f3a862ed5 50 | G01 G11 G21 |
hirokimineshita 0:ad6f3a862ed5 51
hirokimineshita 0:ad6f3a862ed5 52 測定範囲:range (=0のとき1.5G =1のとき6.0G)
hirokimineshita 0:ad6f3a862ed5 53 に設定します。
hirokimineshita 0:ad6f3a862ed5 54 A00～G21の値を def (=-1) にするとその値は変更しません。
hirokimineshita 0:ad6f3a862ed5 55
hirokimineshita 0:ad6f3a862ed5 56
hirokimineshita 0:ad6f3a862ed5 57 name.set_num(&new_mat,range);
hirokimineshita 0:ad6f3a862ed5 58
hirokimineshita 0:ad6f3a862ed5 59 配列 mat に new_mat をうつします。並び方を間違えないように注意してください。
hirokimineshita 0:ad6f3a862ed5 60 new_mat = {A00,A10,A20,A01,A11,A21,G00,G10,G20,G01,G11,G21}としてもいい。
hirokimineshita 0:ad6f3a862ed5 61 測定範囲を range に設定します。
hirokimineshita 0:ad6f3a862ed5 62 A00～G21の値を def (=-1) にすると変更しません。
hirokimineshita 0:ad6f3a862ed5 63
hirokimineshita 0:ad6f3a862ed5 64
hirokimineshita 0:ad6f3a862ed5 65 name.set_range(range);
hirokimineshita 0:ad6f3a862ed5 66
hirokimineshita 0:ad6f3a862ed5 67 測定範囲を range に設定
hirokimineshita 0:ad6f3a862ed5 68
hirokimineshita 0:ad6f3a862ed5 69
hirokimineshita 0:ad6f3a862ed5 70 name.clear_mat();
hirokimineshita 0:ad6f3a862ed5 71
hirokimineshita 0:ad6f3a862ed5 72 配列 mat に mma7361.h で定義された A00 から G21 までを代入します。初期化の際に使われていて、
hirokimineshita 0:ad6f3a862ed5 73 実際のプログラム中では使う必要はありません。
hirokimineshita 0:ad6f3a862ed5 74
hirokimineshita 0:ad6f3a862ed5 75
hirokimineshita 0:ad6f3a862ed5 76 var = name.read_bit(axis);
hirokimineshita 0:ad6f3a862ed5 77
hirokimineshita 0:ad6f3a862ed5 78 var:int型の戻り値(12bit)
hirokimineshita 0:ad6f3a862ed5 79 axis: x_axis (=0)、y_axis (=1)、z_axis (=2) で軸を指定します。
hirokimineshita 0:ad6f3a862ed5 80 指定した軸の加速度をA/D変換したのを何もせずに返します。
hirokimineshita 0:ad6f3a862ed5 81 name.read_u16 を軸指定で簡単にできるようにしただけです。
hirokimineshita 0:ad6f3a862ed5 82
hirokimineshita 0:ad6f3a862ed5 83
hirokimineshita 0:ad6f3a862ed5 84 var = name.read_bit_0(axis);
hirokimineshita 0:ad6f3a862ed5 85
hirokimineshita 0:ad6f3a862ed5 86 var:int型の戻り値
hirokimineshita 0:ad6f3a862ed5 87 axis: x_axis、y_axis、z_axis で軸を指定。
hirokimineshita 0:ad6f3a862ed5 88 指定した軸の加速度をA/D変換したのを配列 mat にしたがって加速度がないときに
hirokimineshita 0:ad6f3a862ed5 89 0になるように引き算したものです。
hirokimineshita 0:ad6f3a862ed5 90
hirokimineshita 0:ad6f3a862ed5 91
hirokimineshita 0:ad6f3a862ed5 92 var = name.read_by_g(axis);
hirokimineshita 0:ad6f3a862ed5 93
hirokimineshita 0:ad6f3a862ed5 94 var:float型の戻り値
hirokimineshita 0:ad6f3a862ed5 95 axis: x_axis、y_axis、z_axis で軸を指定。
hirokimineshita 0:ad6f3a862ed5 96 指定した軸の加速度を配列 mat に従って計算し、何Gかを返します。
hirokimineshita 0:ad6f3a862ed5 97
hirokimineshita 0:ad6f3a862ed5 98
hirokimineshita 0:ad6f3a862ed5 99 name.low_pass_filter(&int_bit,axis);
hirokimineshita 0:ad6f3a862ed5 100
hirokimineshita 0:ad6f3a862ed5 101 int_bit:前回のこの関数で得られたセンサ値(int)を保持している変数。
hirokimineshita 0:ad6f3a862ed5 102 axis: x_axis、y_axis、z_axis で軸を指定。
hirokimineshita 0:ad6f3a862ed5 103 これは擬似的にローパスフィルタをやっています。これによりノイズを消去します。
hirokimineshita 0:ad6f3a862ed5 104 ただ、反応が鈍くなったり、遅くなったりします。変数を1つ用意してください。やってる計算は下の通りです。
hirokimineshita 0:ad6f3a862ed5 105 現在の値 = 0.9 × ひとつ前の値 ＋ 0.1 × センサの値
hirokimineshita 0:ad6f3a862ed5 106 参考:http://android.ohwada.jp/archives/334
hirokimineshita 0:ad6f3a862ed5 107 純粋に加速度を得たときはあまり使わない方がいいかもしれないです。
hirokimineshita 0:ad6f3a862ed5 108 bit値に対して行っており、戻り値は name.read_bit(axis) の値にローパスフィルタを掛けたものになっています。
hirokimineshita 0:ad6f3a862ed5 109 最初は収束するまでに時間がかかるので何百個かのデータをとって捨てるか、後述の関数 name.set_1st_bit() を
hirokimineshita 0:ad6f3a862ed5 110 つかって初期化してください。
hirokimineshita 0:ad6f3a862ed5 111
hirokimineshita 0:ad6f3a862ed5 112
hirokimineshita 0:ad6f3a862ed5 113 name.show_mat();
hirokimineshita 0:ad6f3a862ed5 114 配列 mat をターミナルソフトに表示させます。形は3×4です。
hirokimineshita 0:ad6f3a862ed5 115
hirokimineshita 0:ad6f3a862ed5 116
hirokimineshita 0:ad6f3a862ed5 117 var = name.bit_to_g(bit,axis);
hirokimineshita 0:ad6f3a862ed5 118
hirokimineshita 0:ad6f3a862ed5 119 var:float型の戻り値
hirokimineshita 0:ad6f3a862ed5 120 bit:変換したいデジタル値(int)
hirokimineshita 0:ad6f3a862ed5 121 axis: x_axis、y_axis、z_axis で軸を指定。
hirokimineshita 0:ad6f3a862ed5 122 与えられたデジタル値を配列 mat にしたがって0にあわせ、重力加速度に変換します。
hirokimineshita 0:ad6f3a862ed5 123
hirokimineshita 0:ad6f3a862ed5 124
hirokimineshita 0:ad6f3a862ed5 125 name.set_1st_bit(&var,axis,dir);
hirokimineshita 0:ad6f3a862ed5 126
hirokimineshita 0:ad6f3a862ed5 127 var:int型の変数
hirokimineshita 0:ad6f3a862ed5 128 axis: x_axis、y_axis、z_axis で軸を指定。
hirokimineshita 0:ad6f3a862ed5 129 dir:float型の値。重力加速度を打ち消す加速度がその軸に対してどのくらい働いているか。大まかでいい。(-1～1)
hirokimineshita 0:ad6f3a862ed5 130 ローパスフィルタを使う前にこれを使ってセンサ値を格納する変数を初期化しておくと
hirokimineshita 0:ad6f3a862ed5 131 早くセンサ値に収束します。これを使わなくてもいいですが、フィルタを通した最初の300個ぐらいの値
hirokimineshita 0:ad6f3a862ed5 132 は正しくなくなります。dirの値が正確であればあるほど速く収束します。dirの値が毎回異なるときは
hirokimineshita 0:ad6f3a862ed5 133 0を入れておくことで、収束にかかる時間を減らすことができます。
hirokimineshita 0:ad6f3a862ed5 134
hirokimineshita 0:ad6f3a862ed5 135
hirokimineshita 0:ad6f3a862ed5 136 var = name.rotate(&var_a,a_axis,&var_b,b_axis);
hirokimineshita 0:ad6f3a862ed5 137
hirokimineshita 0:ad6f3a862ed5 138 var:int型の戻り値
hirokimineshita 0:ad6f3a862ed5 139 var_a: aのセンサ値を保管している場所
hirokimineshita 0:ad6f3a862ed5 140 a_axis: aの軸、x_axis、y_axis、z_axis で軸を指定。
hirokimineshita 0:ad6f3a862ed5 141 var_b: bのセンサ値を保管している場所
hirokimineshita 0:ad6f3a862ed5 142 b_axis: bの軸、x_axis、y_axis、z_axis で軸を指定。
hirokimineshita 0:ad6f3a862ed5 143 軸 b から軸 a への回転の角加速度(ラジアン)を計算します。なかで上述の関数 name.bit_to_g() を使っているので
hirokimineshita 0:ad6f3a862ed5 144 そこを考慮してください。並進方向が入ると正しい値を返せません。
hirokimineshita 0:ad6f3a862ed5 145 加速度が0.01Gより小さいときは0Gとしています。軸 b が 0G のときは返る値も0となります。
hirokimineshita 0:ad6f3a862ed5 146
hirokimineshita 0:ad6f3a862ed5 147
hirokimineshita 0:ad6f3a862ed5 148 var = name.rad_to_deg(rad);
hirokimineshita 0:ad6f3a862ed5 149
hirokimineshita 0:ad6f3a862ed5 150 var:float型の戻り値(度)
hirokimineshita 0:ad6f3a862ed5 151 rad:変換したい値(ラジアン)
hirokimineshita 0:ad6f3a862ed5 152 値をラジアンから度に変換してそれを返します。角度、角速度、角加速度に対して使えます。
hirokimineshita 0:ad6f3a862ed5 153 πを初期では3.1415926535にしていますが、mma7361.h のなかで define されている PI の値を変更する
hirokimineshita 1:3541f3ed831d 154 ことで変えることができます。
hirokimineshita 1:3541f3ed831d 155
hirokimineshita 1:3541f3ed831d 156
hirokimineshita 1:3541f3ed831d 157 サンプルプログラム
hirokimineshita 1:3541f3ed831d 158
hirokimineshita 1:3541f3ed831d 159 #include "mbed.h"
hirokimineshita 1:3541f3ed831d 160 #include "mma7361.h"
hirokimineshita 1:3541f3ed831d 161
hirokimineshita 1:3541f3ed831d 162 Serial pc(USBTX,USBRX);
hirokimineshita 1:3541f3ed831d 163
hirokimineshita 1:3541f3ed831d 164 DigitalOut myled(LED1);
hirokimineshita 1:3541f3ed831d 165 mma7361 a;
hirokimineshita 1:3541f3ed831d 166
hirokimineshita 1:3541f3ed831d 167 int main() {
hirokimineshita 1:3541f3ed831d 168 a.clear_mat();
hirokimineshita 1:3541f3ed831d 169 a.show_mat();
hirokimineshita 1:3541f3ed831d 170 int x,y;
hirokimineshita 1:3541f3ed831d 171 a.set_1st_bit(&x,x_axis,0);
hirokimineshita 1:3541f3ed831d 172 a.set_1st_bit(&y,y_axis,0);
hirokimineshita 1:3541f3ed831d 173 float gx,gy,theta;
hirokimineshita 1:3541f3ed831d 174 while(1){
hirokimineshita 1:3541f3ed831d 175 a.low_pass_filter(&x,x_axis);
hirokimineshita 1:3541f3ed831d 176 a.low_pass_filter(&y,y_axis);
hirokimineshita 1:3541f3ed831d 177 gx=a.bit_to_g(x,x_axis);
hirokimineshita 1:3541f3ed831d 178 gy=a.bit_to_g(y,y_axis);
hirokimineshita 1:3541f3ed831d 179 theta=a.rotate(&x,x_axis,&y,y_axis);
hirokimineshita 1:3541f3ed831d 180 theta=a.rad_to_deg(theta);
hirokimineshita 1:3541f3ed831d 181 pc.printf("%.3fG %.3fG %.3f\n\r",gx,gy,theta);
hirokimineshita 1:3541f3ed831d 182 }
hirokimineshita 1:3541f3ed831d 183 }
hirokimineshita 1:3541f3ed831d 184
hirokimineshita 1:3541f3ed831d 185
hirokimineshita 1:3541f3ed831d 186 実行結果
hirokimineshita 1:3541f3ed831d 187
hirokimineshita 1:3541f3ed831d 188 31356 34040 28639
hirokimineshita 1:3541f3ed831d 189 32717 33388 31475
hirokimineshita 1:3541f3ed831d 190 15633 15797 15187
hirokimineshita 1:3541f3ed831d 191 5416 3220 7131
hirokimineshita 1:3541f3ed831d 192 -0.100G -0.130G 37.606
hirokimineshita 1:3541f3ed831d 193 -0.164G -0.233G 35.183
hirokimineshita 1:3541f3ed831d 194 -0.216G -0.321G 33.904
hirokimineshita 1:3541f3ed831d 195 -0.262G -0.401G 33.177
hirokimineshita 1:3541f3ed831d 196 -0.300G -0.470G 32.591
hirokimineshita 1:3541f3ed831d 197 -0.339G -0.535G 32.352
hirokimineshita 1:3541f3ed831d 198 -0.374G -0.593G 32.223
hirokimineshita 1:3541f3ed831d 199 -0.404G -0.645G 32.071
hirokimineshita 2:78a6ee76433c 200 -0.428G -0.689G 31.840
hirokimineshita 2:78a6ee76433c 201 */