![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
edited_01
Fork of 1stOfilter_DNNHL_e02 by
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?
User | Revision | Line number | New 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 | } |