![](/media/cache/group/gazou_0224.jpg.50x50_q85.jpg)
LowpassFilter(仮)?を使って加速度の値をフィルタリングして角度をもとめてます。ローカルファイルシステムを使って値をグラフ化できるようにしてます。 ローパスフィルタについてアドバイスがあればお願いします。
Dependencies: LIS3DH_Hello mbed
Fork of LocalFileSystem_HelloWorld by
main.cpp@3:edc2c71d87b1, 2015-03-30 (annotated)
- Committer:
- tknara
- Date:
- Mon Mar 30 16:05:06 2015 +0000
- Revision:
- 3:edc2c71d87b1
- Parent:
- 2:69e9bf864751
LowPassFilter_test;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbed_official | 0:cc465aef98cf | 1 | #include "mbed.h" |
tknara | 2:69e9bf864751 | 2 | #include "LIS3DH.h"//自作ライブラリ |
tknara | 2:69e9bf864751 | 3 | #define pi 3.141592 |
tknara | 2:69e9bf864751 | 4 | #define filter 0.8//俺的なローパスフィルタの係数一番いい感じなやつ(2015/3/31) |
tknara | 2:69e9bf864751 | 5 | LIS3DH I2c(p9,p10); //sda,scl |
tknara | 2:69e9bf864751 | 6 | Serial pc(USBTX,USBRX); |
tknara | 2:69e9bf864751 | 7 | LocalFileSystem local("local"); |
tknara | 2:69e9bf864751 | 8 | DigitalOut myled(LED4); |
tknara | 2:69e9bf864751 | 9 | //角度を算出する関数(ローパスフィルタを使用) |
tknara | 2:69e9bf864751 | 10 | float angle (float nowx,float nowy) { |
tknara | 3:edc2c71d87b1 | 11 | static float x = 0,y = 0;//前回の値を入れておく変数 |
tknara | 2:69e9bf864751 | 12 | float theta,deg,getx,gety; |
tknara | 3:edc2c71d87b1 | 13 | //filter |
tknara | 2:69e9bf864751 | 14 | getx = filter*x + (1-filter)*nowx; |
tknara | 2:69e9bf864751 | 15 | gety = filter*y + (1-filter)*nowy; |
tknara | 3:edc2c71d87b1 | 16 | //角度を求める[theta] |
tknara | 2:69e9bf864751 | 17 | theta = atan(getx/gety); |
tknara | 3:edc2c71d87b1 | 18 | //変換[deg] |
tknara | 2:69e9bf864751 | 19 | deg = theta * 180 / pi; |
tknara | 3:edc2c71d87b1 | 20 | //今回の値を保存 |
tknara | 2:69e9bf864751 | 21 | x = nowx; |
tknara | 2:69e9bf864751 | 22 | y = nowy; |
tknara | 2:69e9bf864751 | 23 | return deg; |
tknara | 2:69e9bf864751 | 24 | } |
mbed_official | 0:cc465aef98cf | 25 | int main() { |
tknara | 2:69e9bf864751 | 26 | FILE *fp; |
tknara | 2:69e9bf864751 | 27 | //float val; //操作量 |
tknara | 2:69e9bf864751 | 28 | float data[3];//加速度の値を格納 |
tknara | 2:69e9bf864751 | 29 | if ( NULL == (fp = fopen( "/local/test.csv", "w" )) ) |
tknara | 2:69e9bf864751 | 30 | error( "" ); |
tknara | 2:69e9bf864751 | 31 | |
tknara | 2:69e9bf864751 | 32 | myled = 1; |
tknara | 2:69e9bf864751 | 33 | |
tknara | 2:69e9bf864751 | 34 | for ( int i = 0; i < 10000; i++ ) { |
tknara | 2:69e9bf864751 | 35 | int check = I2c.start();//LIS3DHの動作チェック |
tknara | 2:69e9bf864751 | 36 | if (check == 1) { |
tknara | 2:69e9bf864751 | 37 | I2c.read_data(data);//LIS3DHより加速度を入手 |
tknara | 2:69e9bf864751 | 38 | data[3] = angle(data[0],data[1]); |
tknara | 2:69e9bf864751 | 39 | fprintf( fp, "%f\n", data[3] ); |
tknara | 2:69e9bf864751 | 40 | //wait( 0.1 ); |
tknara | 2:69e9bf864751 | 41 | } |
tknara | 2:69e9bf864751 | 42 | } |
tknara | 2:69e9bf864751 | 43 | |
tknara | 2:69e9bf864751 | 44 | fclose( fp ); |
tknara | 2:69e9bf864751 | 45 | myled = 0; |
mbed_official | 0:cc465aef98cf | 46 | } |