mbedを用いた制御学生の制御 / Mbed 2 deprecated LowpassFilter_test

LowpassFilter(仮)?を使って加速度の値をフィルタリングして角度をもとめてます。ローカルファイルシステムを使って値をグラフ化できるようにしてます。 ローパスフィルタについてアドバイスがあればお願いします。

Dependencies:   LIS3DH_Hello mbed

Fork of LocalFileSystem_HelloWorld by mbed official

main.cpp@2:69e9bf864751, 2015-03-30 (annotated)

Committer:
tknara
Date:
Mon Mar 30 15:33:24 2015 +0000
Revision:
2:69e9bf864751
Parent:
1:6bf0bf2bbb31
Child:
3:edc2c71d87b1
Ok

Who changed what in which revision?

UserRevisionLine numberNew 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 2:69e9bf864751 11 static float x = 0,y = 0;
tknara 2:69e9bf864751 12 float theta,deg,getx,gety;
tknara 2:69e9bf864751 13 getx = filter*x + (1-filter)*nowx;
tknara 2:69e9bf864751 14 gety = filter*y + (1-filter)*nowy;
tknara 2:69e9bf864751 15 theta = atan(getx/gety);
tknara 2:69e9bf864751 16 deg = theta * 180 / pi;
tknara 2:69e9bf864751 17 x = nowx;
tknara 2:69e9bf864751 18 y = nowy;
tknara 2:69e9bf864751 19 return deg;
tknara 2:69e9bf864751 20 }
mbed_official 0:cc465aef98cf 21 int main() {
tknara 2:69e9bf864751 22 FILE *fp;
tknara 2:69e9bf864751 23 //float val; //操作量
tknara 2:69e9bf864751 24 float data[3];//加速度の値を格納
tknara 2:69e9bf864751 25 if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )
tknara 2:69e9bf864751 26 error( "" );
tknara 2:69e9bf864751 27
tknara 2:69e9bf864751 28 myled = 1;
tknara 2:69e9bf864751 29
tknara 2:69e9bf864751 30 for ( int i = 0; i < 10000; i++ ) {
tknara 2:69e9bf864751 31 int check = I2c.start();//LIS3DHの動作チェック
tknara 2:69e9bf864751 32 if (check == 1) {
tknara 2:69e9bf864751 33 I2c.read_data(data);//LIS3DHより加速度を入手
tknara 2:69e9bf864751 34 data[3] = angle(data[0],data[1]);
tknara 2:69e9bf864751 35 fprintf( fp, "%f\n", data[3] );
tknara 2:69e9bf864751 36 //wait( 0.1 );
tknara 2:69e9bf864751 37 }
tknara 2:69e9bf864751 38 }
tknara 2:69e9bf864751 39
tknara 2:69e9bf864751 40 fclose( fp );
tknara 2:69e9bf864751 41 myled = 0;
mbed_official 0:cc465aef98cf 42 }