Biorobotics / Mbed 2 deprecated piano_robot

newest version,

Dependencies:   QEI mbed

read_filter_emg.cpp@3:11c2175b4478, 2015-10-26 (annotated)

Committer:
roosbulthuis
Date:
Mon Oct 26 11:42:17 2015 +0000
Revision:
3:11c2175b4478
Parent:
1:5c1fc4e9401a 
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
NickDGreg 0:fc6fa085d591 1 /*
NickDGreg 0:fc6fa085d591 2 * read_filter_emg.cpp
NickDGreg 0:fc6fa085d591 3 *
NickDGreg 0:fc6fa085d591 4 * Created on: Oct 14, 2015
NickDGreg 0:fc6fa085d591 5 * Author: User
NickDGreg 0:fc6fa085d591 6 */
NickDGreg 0:fc6fa085d591 7
NickDGreg 0:fc6fa085d591 8 #include "mbed.h"
NickDGreg 0:fc6fa085d591 9 #include "read_filter_emg.h"
NickDGreg 0:fc6fa085d591 10 //included for fabs() function
NickDGreg 0:fc6fa085d591 11 #include <math.h>
NickDGreg 0:fc6fa085d591 12 //EMG::EMG(AnalogIn aI_, double &v1_, double &v2_)
NickDGreg 0:fc6fa085d591 13
NickDGreg 0:fc6fa085d591 14 //EMG constructor
NickDGreg 1:5c1fc4e9401a 15 EMG::EMG(AnalogIn &aI_, double &v1_, double &v2_)
NickDGreg 0:fc6fa085d591 16 {
NickDGreg 0:fc6fa085d591 17 //initialise the variables of the EMG instance
NickDGreg 0:fc6fa085d591 18 // ?????? NOT SURE HOW THIS SECTION SHOULD WORK EXACTLY, WHAT SHOULD I DEFINE, WHAT SHOULD I 'RENAME'
NickDGreg 0:fc6fa085d591 19 // E.G aI = aI_, v1 = v1_...
NickDGreg 0:fc6fa085d591 20 //
NickDGreg 1:5c1fc4e9401a 21 aI = aI_;
NickDGreg 0:fc6fa085d591 22 /* two past values of v1 and v2 initialised in main are
NickDGreg 0:fc6fa085d591 23 passed to the function as they need to be kept track of
NickDGreg 0:fc6fa085d591 24 cannot be reset each time as filtering would be incorrect
NickDGreg 0:fc6fa085d591 25 */
NickDGreg 0:fc6fa085d591 26 v1 = v1_;
NickDGreg 0:fc6fa085d591 27 v2 = v2_;
NickDGreg 0:fc6fa085d591 28 //initialised to zero, updated in sample()
NickDGreg 0:fc6fa085d591 29 input_sample = 0;
NickDGreg 0:fc6fa085d591 30 //initialised to zero, calculated in filter function using input_sample
NickDGreg 0:fc6fa085d591 31 filtered_sample = 0;
NickDGreg 0:fc6fa085d591 32 };
NickDGreg 0:fc6fa085d591 33
NickDGreg 0:fc6fa085d591 34 void EMG::sample()
NickDGreg 0:fc6fa085d591 35 {
NickDGreg 0:fc6fa085d591 36 //change value of sample to input of the EMG
NickDGreg 0:fc6fa085d591 37 //should i use the reference?
NickDGreg 0:fc6fa085d591 38 input_sample = aI.read();
NickDGreg 0:fc6fa085d591 39 }
NickDGreg 0:fc6fa085d591 40
NickDGreg 0:fc6fa085d591 41 //general biquad filter that can be called in all the filter functions
NickDGreg 0:fc6fa085d591 42 double EMG::biquad(double u, double &v1, double &v2, const double a1,
NickDGreg 0:fc6fa085d591 43 const double a2, const double b0, const double b1, const double b2)
NickDGreg 0:fc6fa085d591 44 {
NickDGreg 0:fc6fa085d591 45 double v = u * a1*v1 * a2*v2;
NickDGreg 0:fc6fa085d591 46 double y = b0*v + b1*v1 + b2*v2;
NickDGreg 0:fc6fa085d591 47 //values of v2 and v1 are updated, as they are passed by reference
NickDGreg 0:fc6fa085d591 48 //they update globally
NickDGreg 0:fc6fa085d591 49 v2 = v1; v1 = v;
NickDGreg 0:fc6fa085d591 50 return y;
NickDGreg 0:fc6fa085d591 51 }
NickDGreg 0:fc6fa085d591 52
NickDGreg 0:fc6fa085d591 53 double EMG::highpass_filter(double input)
NickDGreg 0:fc6fa085d591 54 {
NickDGreg 0:fc6fa085d591 55 /* high pass filter consists of three cascaded biquads
NickDGreg 0:fc6fa085d591 56 blow coefficients for those three biquads */
NickDGreg 0:fc6fa085d591 57 //first high pass biquad
NickDGreg 0:fc6fa085d591 58 const double highp1_a1 = -0.67538034389;
NickDGreg 0:fc6fa085d591 59 const double highp1_a2 = 0.12769255668;
NickDGreg 0:fc6fa085d591 60 const double highp1_b0 = 1.00000000000;
NickDGreg 0:fc6fa085d591 61 const double highp1_b1 = -2.00000000000;
NickDGreg 0:fc6fa085d591 62 const double highp1_b2 = 1.00000000000;
NickDGreg 0:fc6fa085d591 63
NickDGreg 0:fc6fa085d591 64 //second high pass biquad
NickDGreg 0:fc6fa085d591 65 const double highp2_a1 = -0.76475499450;
NickDGreg 0:fc6fa085d591 66 const double highp2_a2 = 0.27692273367;
NickDGreg 0:fc6fa085d591 67 const double highp2_b0 = 1.00000000000;
NickDGreg 0:fc6fa085d591 68 const double highp2_b1 = -2.00000000000;
NickDGreg 0:fc6fa085d591 69 const double highp2_b2 = 1.00000000000;
NickDGreg 0:fc6fa085d591 70
NickDGreg 0:fc6fa085d591 71 //third high pass biquad
NickDGreg 0:fc6fa085d591 72 const double highp3_a1 = -0.99216561242;
NickDGreg 0:fc6fa085d591 73 const double highp3_a2 = 0.65663360837;
NickDGreg 0:fc6fa085d591 74 const double highp3_b0 = 1.00000000000;
NickDGreg 0:fc6fa085d591 75 const double highp3_b1 = -2.00000000000;
NickDGreg 0:fc6fa085d591 76 const double highp3_b2 = 1.00000000000;
NickDGreg 0:fc6fa085d591 77
NickDGreg 0:fc6fa085d591 78 //input to each filter is output of the filter before(excl. first which uses input_sample)
NickDGreg 0:fc6fa085d591 79 /* NOT SURE IF PASSING V1 AND V2 IS CORRECT,
NickDGreg 0:fc6fa085d591 80 WILL IT UPDATE IN THE MEMORY POSITION SO THAT
NickDGreg 0:fc6fa085d591 81 V1 IS CHANGED GLOBALLY */
NickDGreg 0:fc6fa085d591 82 double y1 = EMG::biquad(input, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
NickDGreg 0:fc6fa085d591 83 double y2 = EMG::biquad(y1, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
NickDGreg 0:fc6fa085d591 84 double y3 = EMG::biquad(y2, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
NickDGreg 0:fc6fa085d591 85
NickDGreg 0:fc6fa085d591 86 return y3;
NickDGreg 0:fc6fa085d591 87 }
NickDGreg 0:fc6fa085d591 88
NickDGreg 0:fc6fa085d591 89 //need to implement
NickDGreg 0:fc6fa085d591 90 double rectify(double input)
NickDGreg 0:fc6fa085d591 91 {
NickDGreg 0:fc6fa085d591 92 input = fabs(input);
NickDGreg 0:fc6fa085d591 93 return input;
NickDGreg 0:fc6fa085d591 94 }
NickDGreg 0:fc6fa085d591 95 double EMG::lowpass_filter(double input)
NickDGreg 0:fc6fa085d591 96 {
NickDGreg 0:fc6fa085d591 97 /* lowpass filter consists of three cascaded biquads
NickDGreg 0:fc6fa085d591 98 below the coefficients for those three biquads */
NickDGreg 0:fc6fa085d591 99 //first high pass biquad
NickDGreg 0:fc6fa085d591 100 const double lowp1_a1 = -1.05207469728;
NickDGreg 0:fc6fa085d591 101 const double lowp1_a2 = 0.28586907478;
NickDGreg 0:fc6fa085d591 102 const double lowp1_b0 = 1.00000000000;
NickDGreg 0:fc6fa085d591 103 const double lowp1_b1 = 2.00000000000;
NickDGreg 0:fc6fa085d591 104 const double lowp1_b2 = 1.00000000000;
NickDGreg 0:fc6fa085d591 105
NickDGreg 0:fc6fa085d591 106 //second high pass biquad
NickDGreg 0:fc6fa085d591 107 const double lowp2_a1 = -1.16338171052;
NickDGreg 0:fc6fa085d591 108 const double lowp2_a2 = 0.42191097989;
NickDGreg 0:fc6fa085d591 109 const double lowp2_b0 = 1.00000000000;
NickDGreg 0:fc6fa085d591 110 const double lowp2_b1 = 2.00000000000;
NickDGreg 0:fc6fa085d591 111 const double lowp2_b2 = 1.00000000000;
NickDGreg 0:fc6fa085d591 112
NickDGreg 0:fc6fa085d591 113 //third high pass biquad
NickDGreg 0:fc6fa085d591 114 const double lowp3_a1 = -1.42439823874;
NickDGreg 0:fc6fa085d591 115 const double lowp3_a2 = 0.74093118112;
NickDGreg 0:fc6fa085d591 116 const double lowp3_b0 = 1.00000000000;
NickDGreg 0:fc6fa085d591 117 const double lowp3_b1 = 2.00000000000;
NickDGreg 0:fc6fa085d591 118 const double lowp3_b2 = 1.00000000000;
NickDGreg 0:fc6fa085d591 119
NickDGreg 0:fc6fa085d591 120 //input to each filter is output of the filter before(excl. first which uses input_sample)
NickDGreg 0:fc6fa085d591 121 /* NOT SURE IF PASSING V1 AND V2 IS CORRECT,
NickDGreg 0:fc6fa085d591 122 WILL IT UPDATE IN THE MEMORY POSITION SO THAT V1 IS
NickDGreg 0:fc6fa085d591 123 CHANGED GLOBALLY */
NickDGreg 0:fc6fa085d591 124 double y1 = EMG::biquad(input, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
NickDGreg 0:fc6fa085d591 125 double y2 = EMG::biquad(y1, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
NickDGreg 0:fc6fa085d591 126 double y3 = EMG::biquad(y2, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
NickDGreg 0:fc6fa085d591 127
NickDGreg 0:fc6fa085d591 128 return y3;
NickDGreg 0:fc6fa085d591 129 }
NickDGreg 0:fc6fa085d591 130
NickDGreg 0:fc6fa085d591 131 double EMG::filter(double input)
NickDGreg 0:fc6fa085d591 132 {
NickDGreg 0:fc6fa085d591 133 /* function passes the input through the three filters
NickDGreg 0:fc6fa085d591 134 returns the final output value as filtered sample
NickDGreg 0:fc6fa085d591 135 this is used in check_state() function to determine state of system
NickDGreg 0:fc6fa085d591 136 */
NickDGreg 0:fc6fa085d591 137 double y1 = highpass_filter(input);
NickDGreg 0:fc6fa085d591 138 double y2 = rectify(y1);
NickDGreg 0:fc6fa085d591 139 double y3 = lowpass_filter(y3);
NickDGreg 0:fc6fa085d591 140 filtered_sample = y3;
NickDGreg 0:fc6fa085d591 141
NickDGreg 0:fc6fa085d591 142 return filtered_sample;
NickDGreg 0:fc6fa085d591 143 }
NickDGreg 0:fc6fa085d591 144
NickDGreg 0:fc6fa085d591 145
NickDGreg 0:fc6fa085d591 146
NickDGreg 0:fc6fa085d591 147
NickDGreg 0:fc6fa085d591 148
NickDGreg 0:fc6fa085d591 149