Biorobotics / Mbed 2 deprecated piano_robot

newest version,

Dependencies:   QEI mbed

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?

UserRevisionLine numberNew 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 }