Pascal van Baardwijk / Mbed 2 deprecated EMG_Controller

EMG controller with the new awesome filter

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of EMG_Controller by Leon Klute

emg.h@6:6cb7c0247560, 2016-10-25 (annotated)

Committer:
pbaardwijk
Date:
Tue Oct 25 12:07:53 2016 +0000
Revision:
6:6cb7c0247560
Parent:
3:1d43dd4f37eb
Child:
9:1cb2d5ab51e6
Latest version with a few fixes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NahuelM 2:57523bb4e9c6 1 #include "mbed.h"
NahuelM 2:57523bb4e9c6 2 #include "BiQuad.h"
NahuelM 2:57523bb4e9c6 3 #include "HIDScope.h"
NahuelM 2:57523bb4e9c6 4 //Enum with states
NahuelM 2:57523bb4e9c6 5 enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN};
NahuelM 2:57523bb4e9c6 6
NahuelM 2:57523bb4e9c6 7 //Variable called 'state'
NahuelM 2:57523bb4e9c6 8 states state = STATE_DEFAULT;
NahuelM 2:57523bb4e9c6 9
NahuelM 2:57523bb4e9c6 10 //Creating two scope channels
NahuelM 3:1d43dd4f37eb 11 //HIDScope scope(2);
NahuelM 2:57523bb4e9c6 12
NahuelM 2:57523bb4e9c6 13 //Notch filter
NahuelM 2:57523bb4e9c6 14 BiQuadChain notch_50;
NahuelM 2:57523bb4e9c6 15 BiQuad bq1( 1.00000000000, -1.60956348896, 1.00000000000, -1.40195621505, 0.74203282402);
NahuelM 2:57523bb4e9c6 16 BiQuad bq2( 1.00000000000, -1.60724786352, 1.00000000000, -1.33646101015, 0.85967899264);
NahuelM 2:57523bb4e9c6 17 BiQuad bq3( 1.00000000000, -1.61186693071, 1.00000000000, -1.64415455961, 0.89726621230);
NahuelM 2:57523bb4e9c6 18
NahuelM 2:57523bb4e9c6 19 //High pass filter
NahuelM 2:57523bb4e9c6 20 BiQuadChain high_pass;
NahuelM 2:57523bb4e9c6 21 BiQuad bq4( 1.00000000000, -1.99999967822, 1.00000000000, -1.98388291862, 0.98395921205);
NahuelM 2:57523bb4e9c6 22 BiQuad bq5( 1.00000000000, -1.99999812453, 1.00000000000, -1.99324612474, 0.99332432675);
NahuelM 2:57523bb4e9c6 23
NahuelM 2:57523bb4e9c6 24 //Ticker
NahuelM 2:57523bb4e9c6 25 Ticker emgSampleTicker;
NahuelM 2:57523bb4e9c6 26
NahuelM 2:57523bb4e9c6 27 //Timeout to change state after 5 seconds
NahuelM 2:57523bb4e9c6 28 Timeout change_state;
NahuelM 2:57523bb4e9c6 29
NahuelM 2:57523bb4e9c6 30 //Timeout to change state after 10 seconds
NahuelM 2:57523bb4e9c6 31 Timeout change_state2;
NahuelM 2:57523bb4e9c6 32
pbaardwijk 6:6cb7c0247560 33 //led
pbaardwijk 6:6cb7c0247560 34 DigitalOut led(LED_RED);
pbaardwijk 6:6cb7c0247560 35
NahuelM 2:57523bb4e9c6 36 //Emg input
NahuelM 2:57523bb4e9c6 37 AnalogIn emg0( A0 );
NahuelM 2:57523bb4e9c6 38 AnalogIn emg1( A1 );
NahuelM 2:57523bb4e9c6 39 AnalogIn emg2( A2 );
NahuelM 2:57523bb4e9c6 40
NahuelM 2:57523bb4e9c6 41 bool go_emgSample;
NahuelM 2:57523bb4e9c6 42 bool go_find_minmax;
NahuelM 2:57523bb4e9c6 43 double emg_sample[3];
NahuelM 2:57523bb4e9c6 44 double emg_notch[3];
NahuelM 2:57523bb4e9c6 45 double emg_high_passed[3];
NahuelM 2:57523bb4e9c6 46 double emg_rectified;
NahuelM 2:57523bb4e9c6 47 double min_emg[3];
NahuelM 2:57523bb4e9c6 48 double max_emg[3];
NahuelM 2:57523bb4e9c6 49
NahuelM 2:57523bb4e9c6 50 const int n = 200;
NahuelM 2:57523bb4e9c6 51 int counter = 0;
NahuelM 2:57523bb4e9c6 52 double RMSArray0[n] = {0};
NahuelM 2:57523bb4e9c6 53 double RMSArray1[n] = {0};
NahuelM 2:57523bb4e9c6 54 double RMSArray2[n] = {0};
NahuelM 2:57523bb4e9c6 55 double RMS0;
NahuelM 2:57523bb4e9c6 56 double RMS1;
NahuelM 2:57523bb4e9c6 57 double RMS2;
NahuelM 2:57523bb4e9c6 58 double SumRMS0;
NahuelM 2:57523bb4e9c6 59 double SumRMS1;
NahuelM 2:57523bb4e9c6 60 double SumRMS2;
NahuelM 2:57523bb4e9c6 61
NahuelM 3:1d43dd4f37eb 62 double Norm_EMG_0;
NahuelM 3:1d43dd4f37eb 63 double Norm_EMG_1;
NahuelM 3:1d43dd4f37eb 64 double Norm_EMG_2;
NahuelM 2:57523bb4e9c6 65
NahuelM 2:57523bb4e9c6 66 //count for emg min max
NahuelM 2:57523bb4e9c6 67 int start_calibration = 0;
NahuelM 2:57523bb4e9c6 68
NahuelM 2:57523bb4e9c6 69 void emgSample() {
NahuelM 2:57523bb4e9c6 70 go_emgSample = true;
NahuelM 2:57523bb4e9c6 71 }
NahuelM 2:57523bb4e9c6 72
NahuelM 2:57523bb4e9c6 73 void calibrate() {
pbaardwijk 6:6cb7c0247560 74 state = STATE_CALIBRATION;
pbaardwijk 6:6cb7c0247560 75 led.write(0);
NahuelM 2:57523bb4e9c6 76 }
NahuelM 2:57523bb4e9c6 77
NahuelM 2:57523bb4e9c6 78 void run() {
NahuelM 2:57523bb4e9c6 79 state = STATE_RUN;
pbaardwijk 6:6cb7c0247560 80 led.write(1);
NahuelM 2:57523bb4e9c6 81 }
NahuelM 2:57523bb4e9c6 82
NahuelM 2:57523bb4e9c6 83 void EMG_filter();
NahuelM 2:57523bb4e9c6 84 /*
NahuelM 2:57523bb4e9c6 85 int main() {
NahuelM 2:57523bb4e9c6 86 //combine biquads in biquad chains for notch/high- low-pass filters
NahuelM 2:57523bb4e9c6 87 notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
NahuelM 2:57523bb4e9c6 88 high_pass.add( &bq4 ).add( &bq5 );
NahuelM 2:57523bb4e9c6 89
NahuelM 2:57523bb4e9c6 90 change_state.attach( &calibrate,5);
NahuelM 2:57523bb4e9c6 91 change_state2.attach( &run,10);
NahuelM 2:57523bb4e9c6 92 emgSampleTicker.attach( &emgSample, 0.002);
NahuelM 2:57523bb4e9c6 93 while( true ){
NahuelM 2:57523bb4e9c6 94 if(go_emgSample == true){
NahuelM 2:57523bb4e9c6 95 EMG_filter();
NahuelM 2:57523bb4e9c6 96 }
NahuelM 2:57523bb4e9c6 97 }
NahuelM 2:57523bb4e9c6 98 }
NahuelM 2:57523bb4e9c6 99 */
NahuelM 2:57523bb4e9c6 100
NahuelM 2:57523bb4e9c6 101 void EMG_filter() {
NahuelM 2:57523bb4e9c6 102 if(go_emgSample == true){
NahuelM 2:57523bb4e9c6 103 //read the emg signal
NahuelM 2:57523bb4e9c6 104 emg_sample[0] = emg0.read();
NahuelM 2:57523bb4e9c6 105 emg_sample[1] = emg1.read();
NahuelM 2:57523bb4e9c6 106 emg_sample[2] = emg2.read();
NahuelM 2:57523bb4e9c6 107
NahuelM 2:57523bb4e9c6 108 for (int i = 0; i < 3; i++){
NahuelM 2:57523bb4e9c6 109 //filter out the 50Hz components with a notch filter
NahuelM 2:57523bb4e9c6 110 //emg_notch[i] = notch_50.step(emg_sample[i]);
NahuelM 2:57523bb4e9c6 111
NahuelM 2:57523bb4e9c6 112 //high pass the signal (removing motion artifacts and offset)
NahuelM 2:57523bb4e9c6 113 emg_high_passed[i] = high_pass.step(emg_sample[i]);
NahuelM 2:57523bb4e9c6 114 }
NahuelM 2:57523bb4e9c6 115
NahuelM 2:57523bb4e9c6 116 //Calculating RMS
NahuelM 2:57523bb4e9c6 117 SumRMS0 -= pow(RMSArray0[counter],2);
NahuelM 2:57523bb4e9c6 118 SumRMS1 -= pow(RMSArray1[counter],2);
NahuelM 2:57523bb4e9c6 119 SumRMS2 -= pow(RMSArray2[counter],2);
NahuelM 2:57523bb4e9c6 120
NahuelM 2:57523bb4e9c6 121 RMSArray0[counter] = emg_high_passed[0];
NahuelM 2:57523bb4e9c6 122 RMSArray1[counter] = emg_high_passed[1];
NahuelM 2:57523bb4e9c6 123 RMSArray2[counter] = emg_high_passed[2];
NahuelM 2:57523bb4e9c6 124
NahuelM 2:57523bb4e9c6 125 SumRMS0 += pow(RMSArray0[counter],2);
NahuelM 2:57523bb4e9c6 126 SumRMS1 += pow(RMSArray1[counter],2);
NahuelM 2:57523bb4e9c6 127 SumRMS2 += pow(RMSArray2[counter],2);
NahuelM 2:57523bb4e9c6 128
NahuelM 2:57523bb4e9c6 129 counter++;
NahuelM 2:57523bb4e9c6 130 if (counter == n){
NahuelM 2:57523bb4e9c6 131 counter = 0;
NahuelM 2:57523bb4e9c6 132 }
NahuelM 2:57523bb4e9c6 133
NahuelM 2:57523bb4e9c6 134 RMS0 = sqrt(SumRMS0/n);
NahuelM 2:57523bb4e9c6 135 RMS1 = sqrt(SumRMS1/n);
NahuelM 2:57523bb4e9c6 136 RMS2 = sqrt(SumRMS2/n);
NahuelM 2:57523bb4e9c6 137
NahuelM 2:57523bb4e9c6 138 //Calculating min value and max value of emg signal
NahuelM 2:57523bb4e9c6 139 if(state == STATE_CALIBRATION)
NahuelM 2:57523bb4e9c6 140 {
NahuelM 2:57523bb4e9c6 141 if (start_calibration == 0) {
NahuelM 2:57523bb4e9c6 142 min_emg[0] = RMS0;
NahuelM 2:57523bb4e9c6 143 max_emg[0] = RMS0;
NahuelM 2:57523bb4e9c6 144 min_emg[1] = RMS1;
NahuelM 2:57523bb4e9c6 145 max_emg[1] = RMS1;
NahuelM 2:57523bb4e9c6 146 min_emg[2] = RMS2;
NahuelM 2:57523bb4e9c6 147 max_emg[2] = RMS2;
NahuelM 2:57523bb4e9c6 148 start_calibration++;
NahuelM 2:57523bb4e9c6 149 }
NahuelM 2:57523bb4e9c6 150 else {
NahuelM 2:57523bb4e9c6 151 //finding min and max of emg0
NahuelM 2:57523bb4e9c6 152 if (RMS0 < min_emg[0]) {
NahuelM 2:57523bb4e9c6 153 min_emg[0] = RMS0;
NahuelM 2:57523bb4e9c6 154 }
NahuelM 2:57523bb4e9c6 155 else if (RMS0 > max_emg[0]) {
NahuelM 2:57523bb4e9c6 156 max_emg[0] = RMS0;
NahuelM 2:57523bb4e9c6 157 }
NahuelM 2:57523bb4e9c6 158
NahuelM 2:57523bb4e9c6 159 //finding min and max of emg1
NahuelM 2:57523bb4e9c6 160 if (RMS1 < min_emg[1]) {
NahuelM 2:57523bb4e9c6 161 min_emg[1] = RMS1;
NahuelM 2:57523bb4e9c6 162 }
NahuelM 2:57523bb4e9c6 163 else if (RMS1 > max_emg[1]) {
NahuelM 2:57523bb4e9c6 164 max_emg[1] = RMS1;
NahuelM 2:57523bb4e9c6 165 }
NahuelM 2:57523bb4e9c6 166
NahuelM 2:57523bb4e9c6 167 //finding min and max of emg2
NahuelM 2:57523bb4e9c6 168 if (RMS2 < min_emg[2]) {
NahuelM 2:57523bb4e9c6 169 min_emg[2] = RMS2;
NahuelM 2:57523bb4e9c6 170 }
NahuelM 2:57523bb4e9c6 171 else if (RMS2 > max_emg[2]) {
NahuelM 2:57523bb4e9c6 172 max_emg[2] = RMS2;
NahuelM 2:57523bb4e9c6 173 }
NahuelM 2:57523bb4e9c6 174 }
NahuelM 2:57523bb4e9c6 175 }
NahuelM 2:57523bb4e9c6 176
NahuelM 2:57523bb4e9c6 177 //calculating input_forces for controller
NahuelM 3:1d43dd4f37eb 178 Norm_EMG_0 = (RMS0 - min_emg[0])/(max_emg[0]-min_emg[0]);
NahuelM 3:1d43dd4f37eb 179 Norm_EMG_1 = (RMS1 - min_emg[1])/(max_emg[1]-min_emg[1]);
NahuelM 3:1d43dd4f37eb 180 Norm_EMG_2 = (RMS2 - min_emg[2])/(max_emg[2]-min_emg[2]);
NahuelM 2:57523bb4e9c6 181
NahuelM 2:57523bb4e9c6 182 //Send scope data
NahuelM 3:1d43dd4f37eb 183 // scope.set(0,emg_sample[0]);
NahuelM 3:1d43dd4f37eb 184 //scope.set(1,Norm_EMG_0);
NahuelM 2:57523bb4e9c6 185 //scope.set(2,input_force1);
NahuelM 2:57523bb4e9c6 186 //scope.set(3,input_force2);
NahuelM 3:1d43dd4f37eb 187 //scope.send();
NahuelM 2:57523bb4e9c6 188
NahuelM 2:57523bb4e9c6 189 go_emgSample = false;
NahuelM 2:57523bb4e9c6 190 }
NahuelM 2:57523bb4e9c6 191 }
NahuelM 2:57523bb4e9c6 192