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

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

Dependencies:   LIS3DH_Hello mbed

Fork of LocalFileSystem_HelloWorld by mbed official

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?

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 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 }