velo_filter / Mbed 2 deprecated firstorder

firsrt

Dependencies:   QEI mbed

main.cpp@0:e5ae46b065d3, 2018-07-22 (annotated)

Committer:
NiNiHtayLwin
Date:
Sun Jul 22 09:44:27 2018 +0000
Revision:
0:e5ae46b065d3
first order

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;
NiNiHtayLwin 0:e5ae46b065d3 8 double fc = 0.5;
NiNiHtayLwin 0:e5ae46b065d3 9 double tau = 1/(2*PI*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;
NiNiHtayLwin 0:e5ae46b065d3 13 int main()
NiNiHtayLwin 0:e5ae46b065d3 14 {
NiNiHtayLwin 0:e5ae46b065d3 15 Timer myTime;
NiNiHtayLwin 0:e5ae46b065d3 16 myTime.reset();
NiNiHtayLwin 0:e5ae46b065d3 17 myTime.start();
NiNiHtayLwin 0:e5ae46b065d3 18 pc.baud(57600);
NiNiHtayLwin 0:e5ae46b065d3 19 while(1) {
NiNiHtayLwin 0:e5ae46b065d3 20 now_time = myTime.read_ms()/1000.0;
NiNiHtayLwin 0:e5ae46b065d3 21 samp_time = now_time - prv_time;
NiNiHtayLwin 0:e5ae46b065d3 22 now_x = sin(2*PI*0.5*now_time) + 0.5*sin(2*PI*100*now_time);
NiNiHtayLwin 0:e5ae46b065d3 23 now_y = ( samp_time * now_x + (tau * prv_y) ) / ( samp_time + tau);
NiNiHtayLwin 0:e5ae46b065d3 24
NiNiHtayLwin 0:e5ae46b065d3 25 pc.printf(" %F %F %F %F\r", now_time, samp_time, now_x, now_y);
NiNiHtayLwin 0:e5ae46b065d3 26 printf("\n\r");
NiNiHtayLwin 0:e5ae46b065d3 27 prv_time = now_time;
NiNiHtayLwin 0:e5ae46b065d3 28 prv_y = now_y;
NiNiHtayLwin 0:e5ae46b065d3 29
NiNiHtayLwin 0:e5ae46b065d3 30 }
NiNiHtayLwin 0:e5ae46b065d3 31 }