Biorobotics / Mbed 2 deprecated piano_robot

newest version,

Dependencies:   QEI mbed

read_filter_emg.cpp@0:fc6fa085d591, 2015-10-23 (annotated)

Committer:
NickDGreg
Date:
Fri Oct 23 13:14:57 2015 +0000
Revision:
0:fc6fa085d591
Child:
1:5c1fc4e9401a
move_motor compiles, working on filter.  Cannot pass AnalogIn as input, says no default constructor. Cannot pass as pinName as analogIn declared in constructor cannot be seen  by methods; ;

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 0:fc6fa085d591 15 EMG::EMG(PinName 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 0:fc6fa085d591 21 AnalogIn 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