velo_filter / Mbed 2 deprecated 2ndOfilter

adfadsf

Dependencies:   QEI mbed

main.cpp@1:57f0db59db9f, 2018-07-22 (annotated)

Committer:
MTSAung
Date:
Sun Jul 22 15:22:22 2018 +0000
Revision:
1:57f0db59db9f
Parent:
0:f2a2b5a88616 
second order filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NiNiHtayLwin 0:f2a2b5a88616 1 #include "QEI.h"
NiNiHtayLwin 0:f2a2b5a88616 2 #include "mbed.h"
NiNiHtayLwin 0:f2a2b5a88616 3 Serial pc(USBTX, USBRX);
NiNiHtayLwin 0:f2a2b5a88616 4 double prv_time = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 5 double now_time = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 6 double samp_time = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 7 double now_x = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 8 double now_d = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 9 double prv_d = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 10 double pr2v_d = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 11 double a = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 12 double b = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 13 double c = 0.0;
NiNiHtayLwin 0:f2a2b5a88616 14 double PI = 3.1416;
MTSAung 1:57f0db59db9f 15 double fc = 15.0;
MTSAung 1:57f0db59db9f 16 double C = (2.0*PI*fc)*(2.0*PI*fc);
MTSAung 1:57f0db59db9f 17 double B = (2.0*PI*fc)*sqrt(2.0);
MTSAung 1:57f0db59db9f 18
NiNiHtayLwin 0:f2a2b5a88616 19 int main()
NiNiHtayLwin 0:f2a2b5a88616 20 {
NiNiHtayLwin 0:f2a2b5a88616 21 Timer myTime;
NiNiHtayLwin 0:f2a2b5a88616 22 myTime.reset();
NiNiHtayLwin 0:f2a2b5a88616 23 myTime.start();
NiNiHtayLwin 0:f2a2b5a88616 24 pc.baud(57600);
NiNiHtayLwin 0:f2a2b5a88616 25 while(1) {
NiNiHtayLwin 0:f2a2b5a88616 26 now_time = myTime.read_ms()/1000.0;
NiNiHtayLwin 0:f2a2b5a88616 27 samp_time = now_time - prv_time;
MTSAung 1:57f0db59db9f 28 now_x = sin(2.0*PI*0.5*now_time) + 0.25*sin(2*PI*50.0*now_time);
NiNiHtayLwin 0:f2a2b5a88616 29 a = ((samp_time*samp_time)*C)*now_x;
MTSAung 1:57f0db59db9f 30 b = (2.0+(samp_time*B))*prv_d;
NiNiHtayLwin 0:f2a2b5a88616 31 c = pr2v_d;
MTSAung 1:57f0db59db9f 32 now_d = (a+b-c)/(((samp_time*samp_time)*C)+(samp_time*B)+1.0);
MTSAung 1:57f0db59db9f 33
MTSAung 1:57f0db59db9f 34 pc.printf(" %F %F\r", now_x, now_d);
NiNiHtayLwin 0:f2a2b5a88616 35 printf("\n\r");
NiNiHtayLwin 0:f2a2b5a88616 36 prv_time = now_time;
NiNiHtayLwin 0:f2a2b5a88616 37 prv_d = now_d;
NiNiHtayLwin 0:f2a2b5a88616 38 pr2v_d = prv_d;
NiNiHtayLwin 0:f2a2b5a88616 39 }
NiNiHtayLwin 0:f2a2b5a88616 40 }