Biorobotics
/
piano_robot
newest version,
read_filter_new.cpp@3:11c2175b4478, 2015-10-26 (annotated)
- Committer:
- roosbulthuis
- Date:
- Mon Oct 26 11:42:17 2015 +0000
- Revision:
- 3:11c2175b4478
New file for filters without classes, check if input and output are correct. Should be pasted into main code.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
roosbulthuis | 3:11c2175b4478 | 1 | #include "mbed.h" |
roosbulthuis | 3:11c2175b4478 | 2 | #include "read_filter_emg.h" |
roosbulthuis | 3:11c2175b4478 | 3 | //included for fabs() function |
roosbulthuis | 3:11c2175b4478 | 4 | #include <math.h> |
roosbulthuis | 3:11c2175b4478 | 5 | |
roosbulthuis | 3:11c2175b4478 | 6 | //input (u) = analogin |
roosbulthuis | 3:11c2175b4478 | 7 | |
roosbulthuis | 3:11c2175b4478 | 8 | AnalogIn(/*pin-name*/) = &aI; |
roosbulthuis | 3:11c2175b4478 | 9 | input = aI.read(); |
roosbulthuis | 3:11c2175b4478 | 10 | |
roosbulthuis | 3:11c2175b4478 | 11 | double v1=0; |
roosbulthuis | 3:11c2175b4478 | 12 | double v2=0; |
roosbulthuis | 3:11c2175b4478 | 13 | |
roosbulthuis | 3:11c2175b4478 | 14 | //general biquad filter that can be called in all the filter functions |
roosbulthuis | 3:11c2175b4478 | 15 | double biquad(double u, double &v1, double &v2, const double a1, |
roosbulthuis | 3:11c2175b4478 | 16 | const double a2, const double b0, const double b1, const double b2) |
roosbulthuis | 3:11c2175b4478 | 17 | { |
roosbulthuis | 3:11c2175b4478 | 18 | double v = u * a1*v1 * a2*v2; |
roosbulthuis | 3:11c2175b4478 | 19 | double y = b0*v + b1*v1 + b2*v2; |
roosbulthuis | 3:11c2175b4478 | 20 | //values of v2 and v1 are updated, as they are passed by reference |
roosbulthuis | 3:11c2175b4478 | 21 | //they update globally |
roosbulthuis | 3:11c2175b4478 | 22 | v2 = v1; v1 = v; |
roosbulthuis | 3:11c2175b4478 | 23 | return y; |
roosbulthuis | 3:11c2175b4478 | 24 | } |
roosbulthuis | 3:11c2175b4478 | 25 | |
roosbulthuis | 3:11c2175b4478 | 26 | //Specifying filter coefficients highpass |
roosbulthuis | 3:11c2175b4478 | 27 | |
roosbulthuis | 3:11c2175b4478 | 28 | /* high pass filter consists of three cascaded biquads |
roosbulthuis | 3:11c2175b4478 | 29 | blow coefficients for those three biquads */ |
roosbulthuis | 3:11c2175b4478 | 30 | //first high pass biquad |
roosbulthuis | 3:11c2175b4478 | 31 | const double highp1_a1 = -0.67538034389; |
roosbulthuis | 3:11c2175b4478 | 32 | const double highp1_a2 = 0.12769255668; |
roosbulthuis | 3:11c2175b4478 | 33 | const double highp1_b0 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 34 | const double highp1_b1 = -2.00000000000; |
roosbulthuis | 3:11c2175b4478 | 35 | const double highp1_b2 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 36 | |
roosbulthuis | 3:11c2175b4478 | 37 | //second high pass biquad |
roosbulthuis | 3:11c2175b4478 | 38 | const double highp2_a1 = -0.76475499450; |
roosbulthuis | 3:11c2175b4478 | 39 | const double highp2_a2 = 0.27692273367; |
roosbulthuis | 3:11c2175b4478 | 40 | const double highp2_b0 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 41 | const double highp2_b1 = -2.00000000000; |
roosbulthuis | 3:11c2175b4478 | 42 | const double highp2_b2 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 43 | |
roosbulthuis | 3:11c2175b4478 | 44 | //third high pass biquad |
roosbulthuis | 3:11c2175b4478 | 45 | const double highp3_a1 = -0.99216561242; |
roosbulthuis | 3:11c2175b4478 | 46 | const double highp3_a2 = 0.65663360837; |
roosbulthuis | 3:11c2175b4478 | 47 | const double highp3_b0 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 48 | const double highp3_b1 = -2.00000000000; |
roosbulthuis | 3:11c2175b4478 | 49 | const double highp3_b2 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 50 | |
roosbulthuis | 3:11c2175b4478 | 51 | //Specifying filter coefficients lowpass |
roosbulthuis | 3:11c2175b4478 | 52 | |
roosbulthuis | 3:11c2175b4478 | 53 | /* lowpass filter consists of three cascaded biquads |
roosbulthuis | 3:11c2175b4478 | 54 | below the coefficients for those three biquads */ |
roosbulthuis | 3:11c2175b4478 | 55 | //first high pass biquad |
roosbulthuis | 3:11c2175b4478 | 56 | const double lowp1_a1 = -1.05207469728; |
roosbulthuis | 3:11c2175b4478 | 57 | const double lowp1_a2 = 0.28586907478; |
roosbulthuis | 3:11c2175b4478 | 58 | const double lowp1_b0 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 59 | const double lowp1_b1 = 2.00000000000; |
roosbulthuis | 3:11c2175b4478 | 60 | const double lowp1_b2 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 61 | |
roosbulthuis | 3:11c2175b4478 | 62 | //second high pass biquad |
roosbulthuis | 3:11c2175b4478 | 63 | const double lowp2_a1 = -1.16338171052; |
roosbulthuis | 3:11c2175b4478 | 64 | const double lowp2_a2 = 0.42191097989; |
roosbulthuis | 3:11c2175b4478 | 65 | const double lowp2_b0 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 66 | const double lowp2_b1 = 2.00000000000; |
roosbulthuis | 3:11c2175b4478 | 67 | const double lowp2_b2 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 68 | |
roosbulthuis | 3:11c2175b4478 | 69 | //third high pass biquad |
roosbulthuis | 3:11c2175b4478 | 70 | const double lowp3_a1 = -1.42439823874; |
roosbulthuis | 3:11c2175b4478 | 71 | const double lowp3_a2 = 0.74093118112; |
roosbulthuis | 3:11c2175b4478 | 72 | const double lowp3_b0 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 73 | const double lowp3_b1 = 2.00000000000; |
roosbulthuis | 3:11c2175b4478 | 74 | const double lowp3_b2 = 1.00000000000; |
roosbulthuis | 3:11c2175b4478 | 75 | |
roosbulthuis | 3:11c2175b4478 | 76 | //input to each filter is output of the filter before(excl. first which uses input_sample) |
roosbulthuis | 3:11c2175b4478 | 77 | /* NOT SURE IF PASSING V1 AND V2 IS CORRECT, |
roosbulthuis | 3:11c2175b4478 | 78 | WILL IT UPDATE IN THE MEMORY POSITION SO THAT |
roosbulthuis | 3:11c2175b4478 | 79 | V1 IS CHANGED GLOBALLY */ |
roosbulthuis | 3:11c2175b4478 | 80 | |
roosbulthuis | 3:11c2175b4478 | 81 | //highpass |
roosbulthuis | 3:11c2175b4478 | 82 | |
roosbulthuis | 3:11c2175b4478 | 83 | double highpass_filter(double input) |
roosbulthuis | 3:11c2175b4478 | 84 | { |
roosbulthuis | 3:11c2175b4478 | 85 | double y1 = biquad(input, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2); |
roosbulthuis | 3:11c2175b4478 | 86 | double y2 = biquad(y1, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2); |
roosbulthuis | 3:11c2175b4478 | 87 | double y3 = biquad(y2, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2); |
roosbulthuis | 3:11c2175b4478 | 88 | |
roosbulthuis | 3:11c2175b4478 | 89 | return y3; |
roosbulthuis | 3:11c2175b4478 | 90 | } |
roosbulthuis | 3:11c2175b4478 | 91 | |
roosbulthuis | 3:11c2175b4478 | 92 | |
roosbulthuis | 3:11c2175b4478 | 93 | //rectifier |
roosbulthuis | 3:11c2175b4478 | 94 | double rectify(double y3) |
roosbulthuis | 3:11c2175b4478 | 95 | { |
roosbulthuis | 3:11c2175b4478 | 96 | y3 = fabs(y3); |
roosbulthuis | 3:11c2175b4478 | 97 | return y3; |
roosbulthuis | 3:11c2175b4478 | 98 | } |
roosbulthuis | 3:11c2175b4478 | 99 | |
roosbulthuis | 3:11c2175b4478 | 100 | //lowpass |
roosbulthuis | 3:11c2175b4478 | 101 | |
roosbulthuis | 3:11c2175b4478 | 102 | double lowpass_filter(double y3) |
roosbulthuis | 3:11c2175b4478 | 103 | { |
roosbulthuis | 3:11c2175b4478 | 104 | double y4 = biquad(y3, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2); |
roosbulthuis | 3:11c2175b4478 | 105 | double y5 = biquad(y4, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2); |
roosbulthuis | 3:11c2175b4478 | 106 | double filtered_signal = biquad(y5, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2); |
roosbulthuis | 3:11c2175b4478 | 107 | |
roosbulthuis | 3:11c2175b4478 | 108 | return filtered_signal; |
roosbulthuis | 3:11c2175b4478 | 109 | } |
roosbulthuis | 3:11c2175b4478 | 110 | |
roosbulthuis | 3:11c2175b4478 | 111 | double filter(double input) |
roosbulthuis | 3:11c2175b4478 | 112 | { |
roosbulthuis | 3:11c2175b4478 | 113 | /* function passes the input through the three filters |
roosbulthuis | 3:11c2175b4478 | 114 | returns the final output value as filtered sample |
roosbulthuis | 3:11c2175b4478 | 115 | this is used in check_state() function to determine state of system |
roosbulthuis | 3:11c2175b4478 | 116 | */ |
roosbulthuis | 3:11c2175b4478 | 117 | double y1 = highpass_filter(input); |
roosbulthuis | 3:11c2175b4478 | 118 | double y2 = rectify(y3); |
roosbulthuis | 3:11c2175b4478 | 119 | double filtered_signal = lowpass_filter(y3); |
roosbulthuis | 3:11c2175b4478 | 120 | |
roosbulthuis | 3:11c2175b4478 | 121 | return filtered_signal; |
roosbulthuis | 3:11c2175b4478 | 122 | } |