![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
This is the program for 1st order LPF for velocity
main.cpp@0:5da774f9c59c, 2018-07-20 (annotated)
- Committer:
- MTSAung
- Date:
- Fri Jul 20 11:16:46 2018 +0000
- Revision:
- 0:5da774f9c59c
- Child:
- 1:4088422c9f8a
kkk
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MTSAung | 0:5da774f9c59c | 1 | #include "QEI.h" |
MTSAung | 0:5da774f9c59c | 2 | #include "mbed.h" |
MTSAung | 0:5da774f9c59c | 3 | Serial pc(USBTX, USBRX); |
MTSAung | 0:5da774f9c59c | 4 | |
MTSAung | 0:5da774f9c59c | 5 | long counts_per_rev = (1440); |
MTSAung | 0:5da774f9c59c | 6 | double prv_ang = 0.0; |
MTSAung | 0:5da774f9c59c | 7 | double now_ang = 0.0; |
MTSAung | 0:5da774f9c59c | 8 | double now_omg = 0.0; |
MTSAung | 0:5da774f9c59c | 9 | double prv_time = 0.0; |
MTSAung | 0:5da774f9c59c | 10 | double now_time = 0.0; |
MTSAung | 0:5da774f9c59c | 11 | double samp_time = 0.0; |
MTSAung | 0:5da774f9c59c | 12 | double now_x = 0.0; |
MTSAung | 0:5da774f9c59c | 13 | double now_y = 0.0; |
MTSAung | 0:5da774f9c59c | 14 | |
MTSAung | 0:5da774f9c59c | 15 | QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING); |
MTSAung | 0:5da774f9c59c | 16 | |
MTSAung | 0:5da774f9c59c | 17 | float pulsesToDegrees(float pulses) |
MTSAung | 0:5da774f9c59c | 18 | { |
MTSAung | 0:5da774f9c59c | 19 | return ((pulses/counts_per_rev)*360); |
MTSAung | 0:5da774f9c59c | 20 | } |
MTSAung | 0:5da774f9c59c | 21 | |
MTSAung | 0:5da774f9c59c | 22 | int main() |
MTSAung | 0:5da774f9c59c | 23 | { |
MTSAung | 0:5da774f9c59c | 24 | Timer myTime; |
MTSAung | 0:5da774f9c59c | 25 | myTime.reset(); |
MTSAung | 0:5da774f9c59c | 26 | myTime.start(); |
MTSAung | 0:5da774f9c59c | 27 | while(1) { |
MTSAung | 0:5da774f9c59c | 28 | now_time = myTime.read_ms()/1000.0; |
MTSAung | 0:5da774f9c59c | 29 | samp_time = now_time - prv_time; |
MTSAung | 0:5da774f9c59c | 30 | now_ang = pulsesToDegrees(wheel.getPulses()); |
MTSAung | 0:5da774f9c59c | 31 | now_omg = (now_ang - prv_ang)/samp_time; |
MTSAung | 0:5da774f9c59c | 32 | |
MTSAung | 0:5da774f9c59c | 33 | pc.printf(" %F %F \r", now_time, now_ang); |
MTSAung | 0:5da774f9c59c | 34 | printf("\n\r"); |
MTSAung | 0:5da774f9c59c | 35 | prv_ang = now_ang; |
MTSAung | 0:5da774f9c59c | 36 | prv_time = now_time; |
MTSAung | 0:5da774f9c59c | 37 | } |
MTSAung | 0:5da774f9c59c | 38 | } |