velo_filter / Mbed 2 deprecated my1stOLPFilter

This is the program for 1st order LPF for velocity

Dependencies:   QEI mbed

main.cpp@1:4088422c9f8a, 2018-07-29 (annotated)

Committer:
k_waihlyan
Date:
Sun Jul 29 08:54:36 2018 +0000
Revision:
1:4088422c9f8a
Parent:
0:5da774f9c59c 
update_29july

Who changed what in which revision?

UserRevisionLine numberNew 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
k_waihlyan 1:4088422c9f8a 5 AnalogOut pwm (p18);
k_waihlyan 1:4088422c9f8a 6 DigitalOut pin_a (p19);
k_waihlyan 1:4088422c9f8a 7 DigitalOut pin_b (p20);
k_waihlyan 1:4088422c9f8a 8 DigitalIn IRpin(p16);
k_waihlyan 1:4088422c9f8a 9 DigitalIn IRpin2(p17);
k_waihlyan 1:4088422c9f8a 10 int ir,ir2;
k_waihlyan 1:4088422c9f8a 11 long counts_per_rev = (64);
MTSAung 0:5da774f9c59c 12 double prv_ang = 0.0;
k_waihlyan 1:4088422c9f8a 13 double now_ang;
MTSAung 0:5da774f9c59c 14 double now_omg = 0.0;
MTSAung 0:5da774f9c59c 15 double prv_time = 0.0;
MTSAung 0:5da774f9c59c 16 double now_time = 0.0;
MTSAung 0:5da774f9c59c 17 double samp_time = 0.0;
k_waihlyan 1:4088422c9f8a 18 //double now_x = 0.0;
k_waihlyan 1:4088422c9f8a 19 //double now_y = 0.0;
MTSAung 0:5da774f9c59c 20
MTSAung 0:5da774f9c59c 21 QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING);
MTSAung 0:5da774f9c59c 22
MTSAung 0:5da774f9c59c 23 float pulsesToDegrees(float pulses)
MTSAung 0:5da774f9c59c 24 {
MTSAung 0:5da774f9c59c 25 return ((pulses/counts_per_rev)*360);
MTSAung 0:5da774f9c59c 26 }
MTSAung 0:5da774f9c59c 27
MTSAung 0:5da774f9c59c 28 int main()
MTSAung 0:5da774f9c59c 29 {
MTSAung 0:5da774f9c59c 30 Timer myTime;
MTSAung 0:5da774f9c59c 31 myTime.reset();
MTSAung 0:5da774f9c59c 32 myTime.start();
k_waihlyan 1:4088422c9f8a 33 pc.baud(57600);
MTSAung 0:5da774f9c59c 34 while(1) {
k_waihlyan 1:4088422c9f8a 35 ir = IRpin.read();
k_waihlyan 1:4088422c9f8a 36 ir2 = IRpin2.read();
MTSAung 0:5da774f9c59c 37 now_time = myTime.read_ms()/1000.0;
MTSAung 0:5da774f9c59c 38 samp_time = now_time - prv_time;
MTSAung 0:5da774f9c59c 39 now_ang = pulsesToDegrees(wheel.getPulses());
MTSAung 0:5da774f9c59c 40 now_omg = (now_ang - prv_ang)/samp_time;
k_waihlyan 1:4088422c9f8a 41 if(ir == 0 && ir2 == 1)
k_waihlyan 1:4088422c9f8a 42 {
k_waihlyan 1:4088422c9f8a 43 pwm = 0.8 ;
k_waihlyan 1:4088422c9f8a 44 pin_a = 1;
k_waihlyan 1:4088422c9f8a 45 pin_b = 0;
k_waihlyan 1:4088422c9f8a 46 }
k_waihlyan 1:4088422c9f8a 47 else if(ir == 1 && ir2 == 0)
k_waihlyan 1:4088422c9f8a 48 {
k_waihlyan 1:4088422c9f8a 49 pwm = 0.8 ;
k_waihlyan 1:4088422c9f8a 50 pin_a = 0;
k_waihlyan 1:4088422c9f8a 51 pin_b = 1;
k_waihlyan 1:4088422c9f8a 52 }
k_waihlyan 1:4088422c9f8a 53 else
k_waihlyan 1:4088422c9f8a 54 {
k_waihlyan 1:4088422c9f8a 55 pwm = 0.8 ;
k_waihlyan 1:4088422c9f8a 56 pin_a = 0;
k_waihlyan 1:4088422c9f8a 57 pin_b = 0;
k_waihlyan 1:4088422c9f8a 58 }
k_waihlyan 1:4088422c9f8a 59 pc.printf("%d %d %f\r",ir,ir2,now_omg);
MTSAung 0:5da774f9c59c 60 printf("\n\r");
MTSAung 0:5da774f9c59c 61 prv_ang = now_ang;
MTSAung 0:5da774f9c59c 62 prv_time = now_time;
MTSAung 0:5da774f9c59c 63 }
MTSAung 0:5da774f9c59c 64 }