velo_filter / Mbed 2 deprecated 1stOfilter_DNNHLe01

edited_01

Dependencies:   QEI mbed

Fork of 1stOfilter_DNNHL_e02 by velo_filter

main.cpp@1:36f8de3d85bb, 2018-07-22 (annotated)

Committer:
MTSAung
Date:
Sun Jul 22 14:38:12 2018 +0000
Revision:
1:36f8de3d85bb
Parent:
0:e5ae46b065d3 
second order low pass filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NiNiHtayLwin 0:e5ae46b065d3 1 #include "QEI.h"
NiNiHtayLwin 0:e5ae46b065d3 2 #include "mbed.h"
NiNiHtayLwin 0:e5ae46b065d3 3 Serial pc(USBTX, USBRX);
NiNiHtayLwin 0:e5ae46b065d3 4 double prv_time = 0.0;
NiNiHtayLwin 0:e5ae46b065d3 5 double now_time = 0.0;
NiNiHtayLwin 0:e5ae46b065d3 6 double samp_time = 0.0;
NiNiHtayLwin 0:e5ae46b065d3 7 double PI = 3.1416;
MTSAung 1:36f8de3d85bb 8 double fc = 2.0*PI*5.0;
MTSAung 1:36f8de3d85bb 9 double tau = 1/(fc);
NiNiHtayLwin 0:e5ae46b065d3 10 double now_x = 0.0;
NiNiHtayLwin 0:e5ae46b065d3 11 double now_y = 0.0;
NiNiHtayLwin 0:e5ae46b065d3 12 double prv_y = 0.0;
MTSAung 1:36f8de3d85bb 13
NiNiHtayLwin 0:e5ae46b065d3 14 int main()
NiNiHtayLwin 0:e5ae46b065d3 15 {
NiNiHtayLwin 0:e5ae46b065d3 16 Timer myTime;
NiNiHtayLwin 0:e5ae46b065d3 17 myTime.reset();
NiNiHtayLwin 0:e5ae46b065d3 18 myTime.start();
NiNiHtayLwin 0:e5ae46b065d3 19 pc.baud(57600);
NiNiHtayLwin 0:e5ae46b065d3 20 while(1) {
NiNiHtayLwin 0:e5ae46b065d3 21 now_time = myTime.read_ms()/1000.0;
NiNiHtayLwin 0:e5ae46b065d3 22 samp_time = now_time - prv_time;
MTSAung 1:36f8de3d85bb 23 now_x = sin(2.0*PI*0.5*now_time) + 0.25*sin(2.0*PI*50.0*now_time);
NiNiHtayLwin 0:e5ae46b065d3 24 now_y = ( samp_time * now_x + (tau * prv_y) ) / ( samp_time + tau);
NiNiHtayLwin 0:e5ae46b065d3 25
MTSAung 1:36f8de3d85bb 26 pc.printf(" %F %F\r", now_x, now_y);
NiNiHtayLwin 0:e5ae46b065d3 27 printf("\n\r");
NiNiHtayLwin 0:e5ae46b065d3 28 prv_time = now_time;
NiNiHtayLwin 0:e5ae46b065d3 29 prv_y = now_y;
NiNiHtayLwin 0:e5ae46b065d3 30
NiNiHtayLwin 0:e5ae46b065d3 31 }
NiNiHtayLwin 0:e5ae46b065d3 32 }