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

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

Dependencies:   LIS3DH_Hello mbed

Fork of LocalFileSystem_HelloWorld by mbed official

main.cpp

Committer:
tknara
Date:
2015-03-30
Revision:
3:edc2c71d87b1
Parent:
2:69e9bf864751

File content as of revision 3:edc2c71d87b1:

#include "mbed.h"
#include "LIS3DH.h"//自作ライブラリ
#define pi 3.141592
#define filter 0.8//俺的なローパスフィルタの係数一番いい感じなやつ(2015/3/31)
LIS3DH I2c(p9,p10); //sda,scl
Serial pc(USBTX,USBRX);
LocalFileSystem local("local");
DigitalOut myled(LED4);
//角度を算出する関数(ローパスフィルタを使用)
float angle (float nowx,float nowy) {
    static float x = 0,y = 0;//前回の値を入れておく変数
    float theta,deg,getx,gety;
    //filter
    getx = filter*x + (1-filter)*nowx;
    gety = filter*y + (1-filter)*nowy;
    //角度を求める[theta]
    theta = atan(getx/gety);
    //変換[deg]
    deg = theta * 180 / pi;
    //今回の値を保存
    x = nowx;
    y = nowy;
    return deg;
}
int main() {
    FILE *fp;
    //float val; //操作量
    float data[3];//加速度の値を格納
    if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )
        error( "" );

    myled   = 1;

    for ( int i = 0; i < 10000; i++ ) {
        int check = I2c.start();//LIS3DHの動作チェック
        if (check  == 1) {
            I2c.read_data(data);//LIS3DHより加速度を入手
            data[3] = angle(data[0],data[1]);
            fprintf( fp, "%f\n", data[3] );
            //wait( 0.1 );
        }
    }

    fclose( fp );
    myled   = 0;
}