Nahuel Manterola / Mbed 2 deprecated EMG_Controller_5

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of EMG_Controller by Pascal van Baardwijk

emg.h@12:12c162dc8893, 2016-10-28 (annotated)

Committer:
NahuelM
Date:
Fri Oct 28 11:46:44 2016 +0000
Revision:
12:12c162dc8893
Parent:
11:c8b6a2b314c3 
Werkende Arm heen en weer

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 11:c8b6a2b314c3 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 11:c8b6a2b314c3 11 //HIDScope scope(3);
NahuelM 2:57523bb4e9c6 12
NahuelM 2:57523bb4e9c6 13 //Notch filter
NahuelM 12:12c162dc8893 14 BiQuadChain notch_50_0;
NahuelM 12:12c162dc8893 15 BiQuadChain notch_50_1;
NahuelM 12:12c162dc8893 16 BiQuadChain notch_50_2;
NahuelM 12:12c162dc8893 17 BiQuad bq03( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
NahuelM 12:12c162dc8893 18 BiQuad bq04( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
NahuelM 12:12c162dc8893 19 BiQuad bq05( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
NahuelM 12:12c162dc8893 20 BiQuad bq13( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
NahuelM 12:12c162dc8893 21 BiQuad bq14( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
NahuelM 12:12c162dc8893 22 BiQuad bq15( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
NahuelM 12:12c162dc8893 23 BiQuad bq23( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
NahuelM 12:12c162dc8893 24 BiQuad bq24( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
NahuelM 12:12c162dc8893 25 BiQuad bq25( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
NahuelM 2:57523bb4e9c6 26
NahuelM 2:57523bb4e9c6 27 //High pass filter
NahuelM 12:12c162dc8893 28 BiQuadChain high_pass_0;
NahuelM 12:12c162dc8893 29 BiQuadChain high_pass_1;
NahuelM 12:12c162dc8893 30 BiQuadChain high_pass_2;
NahuelM 12:12c162dc8893 31 BiQuad bq06( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
NahuelM 12:12c162dc8893 32 BiQuad bq07( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
NahuelM 12:12c162dc8893 33 BiQuad bq16( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
NahuelM 12:12c162dc8893 34 BiQuad bq17( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
NahuelM 12:12c162dc8893 35 BiQuad bq26( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
NahuelM 12:12c162dc8893 36 BiQuad bq27( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
pbaardwijk 9:1cb2d5ab51e6 37 //BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620);
NahuelM 2:57523bb4e9c6 38
pbaardwijk 9:1cb2d5ab51e6 39 //Low pass filter
NahuelM 12:12c162dc8893 40 BiQuadChain low_pass_0;
NahuelM 12:12c162dc8893 41 BiQuadChain low_pass_1;
NahuelM 12:12c162dc8893 42 BiQuadChain low_pass_2;
pbaardwijk 9:1cb2d5ab51e6 43 BiQuad bq9( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
pbaardwijk 9:1cb2d5ab51e6 44 BiQuad bq10( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
pbaardwijk 9:1cb2d5ab51e6 45 BiQuad bq11( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
NahuelM 12:12c162dc8893 46 BiQuad bq19( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
NahuelM 12:12c162dc8893 47 BiQuad bq110( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
NahuelM 12:12c162dc8893 48 BiQuad bq111( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
NahuelM 12:12c162dc8893 49 BiQuad bq29( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
NahuelM 12:12c162dc8893 50 BiQuad bq210( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
NahuelM 12:12c162dc8893 51 BiQuad bq211( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
NahuelM 12:12c162dc8893 52
NahuelM 2:57523bb4e9c6 53 //Ticker
NahuelM 2:57523bb4e9c6 54 Ticker emgSampleTicker;
NahuelM 2:57523bb4e9c6 55
NahuelM 2:57523bb4e9c6 56 //Timeout to change state after 5 seconds
NahuelM 2:57523bb4e9c6 57 Timeout change_state;
NahuelM 2:57523bb4e9c6 58
pbaardwijk 9:1cb2d5ab51e6 59 //Timeout to change state after 15 seconds
NahuelM 2:57523bb4e9c6 60 Timeout change_state2;
NahuelM 2:57523bb4e9c6 61
pbaardwijk 9:1cb2d5ab51e6 62 //LED
pbaardwijk 6:6cb7c0247560 63 DigitalOut led(LED_RED);
pbaardwijk 6:6cb7c0247560 64
NahuelM 2:57523bb4e9c6 65 //Emg input
NahuelM 2:57523bb4e9c6 66 AnalogIn emg0( A0 );
NahuelM 2:57523bb4e9c6 67 AnalogIn emg1( A1 );
NahuelM 2:57523bb4e9c6 68 AnalogIn emg2( A2 );
NahuelM 2:57523bb4e9c6 69
pbaardwijk 9:1cb2d5ab51e6 70 //Go flag for emg sample
NahuelM 2:57523bb4e9c6 71 bool go_emgSample;
pbaardwijk 9:1cb2d5ab51e6 72
pbaardwijk 9:1cb2d5ab51e6 73 //Variables for intermediate filter values
NahuelM 2:57523bb4e9c6 74 double emg_sample[3];
pbaardwijk 9:1cb2d5ab51e6 75 double emg_low_passed_200[3];
NahuelM 2:57523bb4e9c6 76 double emg_notch[3];
NahuelM 2:57523bb4e9c6 77 double emg_high_passed[3];
pbaardwijk 9:1cb2d5ab51e6 78 double emg_low_passed[3];
NahuelM 2:57523bb4e9c6 79 double min_emg[3];
NahuelM 2:57523bb4e9c6 80 double max_emg[3];
NahuelM 2:57523bb4e9c6 81
pbaardwijk 9:1cb2d5ab51e6 82 //Calculating normalized outputs
NahuelM 3:1d43dd4f37eb 83 double Norm_EMG_0;
NahuelM 3:1d43dd4f37eb 84 double Norm_EMG_1;
NahuelM 3:1d43dd4f37eb 85 double Norm_EMG_2;
NahuelM 2:57523bb4e9c6 86
NahuelM 2:57523bb4e9c6 87 //count for emg min max
NahuelM 2:57523bb4e9c6 88 int start_calibration = 0;
NahuelM 2:57523bb4e9c6 89
NahuelM 2:57523bb4e9c6 90 void emgSample() {
NahuelM 2:57523bb4e9c6 91 go_emgSample = true;
NahuelM 2:57523bb4e9c6 92 }
NahuelM 2:57523bb4e9c6 93
NahuelM 2:57523bb4e9c6 94 void calibrate() {
pbaardwijk 9:1cb2d5ab51e6 95 state = STATE_CALIBRATION;
pbaardwijk 9:1cb2d5ab51e6 96 led.write(0);
NahuelM 2:57523bb4e9c6 97 }
NahuelM 2:57523bb4e9c6 98
NahuelM 2:57523bb4e9c6 99 void run() {
NahuelM 2:57523bb4e9c6 100 state = STATE_RUN;
pbaardwijk 6:6cb7c0247560 101 led.write(1);
NahuelM 2:57523bb4e9c6 102 }
NahuelM 2:57523bb4e9c6 103
NahuelM 2:57523bb4e9c6 104 void EMG_filter();
NahuelM 2:57523bb4e9c6 105
NahuelM 2:57523bb4e9c6 106 void EMG_filter() {
NahuelM 2:57523bb4e9c6 107 if(go_emgSample == true){
NahuelM 12:12c162dc8893 108 //read the emg signal
NahuelM 2:57523bb4e9c6 109 emg_sample[0] = emg0.read();
NahuelM 2:57523bb4e9c6 110 emg_sample[1] = emg1.read();
NahuelM 2:57523bb4e9c6 111 emg_sample[2] = emg2.read();
NahuelM 12:12c162dc8893 112 //pc.printf("%f - %f - %f \r\n", emg_sample[0], emg_sample[1], emg_sample[2]);
pbaardwijk 9:1cb2d5ab51e6 113 //filter out the 50Hz components with a notch filter
NahuelM 12:12c162dc8893 114 emg_notch[0] = notch_50_0.step(emg_sample[0]);
NahuelM 12:12c162dc8893 115 emg_notch[1] = notch_50_1.step(emg_sample[1]);
NahuelM 12:12c162dc8893 116 emg_notch[2] = notch_50_2.step(emg_sample[2]);
pbaardwijk 9:1cb2d5ab51e6 117 //high pass the signal (removing motion artifacts and offset)
NahuelM 12:12c162dc8893 118 emg_high_passed[0] = high_pass_0.step(emg_notch[0]);
NahuelM 12:12c162dc8893 119 emg_high_passed[1] = high_pass_1.step(emg_notch[1]);
NahuelM 12:12c162dc8893 120 emg_high_passed[2] = high_pass_2.step(emg_notch[2]);
NahuelM 12:12c162dc8893 121 float emg_fabs[3];
NahuelM 12:12c162dc8893 122 emg_fabs[0] = fabs(emg_high_passed[0]);
NahuelM 12:12c162dc8893 123 emg_fabs[1] = fabs(emg_high_passed[1]);
NahuelM 12:12c162dc8893 124 emg_fabs[2] = fabs(emg_high_passed[2]);
pbaardwijk 9:1cb2d5ab51e6 125 //low pass the rectified emg signal
NahuelM 12:12c162dc8893 126 emg_low_passed[0] = low_pass_0.step(emg_fabs[0]);
NahuelM 12:12c162dc8893 127 emg_low_passed[1] = low_pass_1.step(emg_fabs[1]);
NahuelM 12:12c162dc8893 128 emg_low_passed[2] = low_pass_2.step(emg_fabs[2]);
NahuelM 2:57523bb4e9c6 129
NahuelM 12:12c162dc8893 130 //pc.printf("%f - %f - %f \r\n", emg_low_passed[0], emg_low_passed[1], emg_low_passed[2]);
NahuelM 2:57523bb4e9c6 131 //Calculating min value and max value of emg signal
NahuelM 2:57523bb4e9c6 132 if(state == STATE_CALIBRATION)
NahuelM 2:57523bb4e9c6 133 {
NahuelM 2:57523bb4e9c6 134 if (start_calibration == 0) {
pbaardwijk 9:1cb2d5ab51e6 135 min_emg[0] = emg_low_passed[0];
pbaardwijk 9:1cb2d5ab51e6 136 max_emg[0] = emg_low_passed[0];
pbaardwijk 9:1cb2d5ab51e6 137 min_emg[1] = emg_low_passed[1];
pbaardwijk 9:1cb2d5ab51e6 138 max_emg[1] = emg_low_passed[1];
pbaardwijk 9:1cb2d5ab51e6 139 min_emg[2] = emg_low_passed[2];
pbaardwijk 9:1cb2d5ab51e6 140 max_emg[2] = emg_low_passed[2];
NahuelM 2:57523bb4e9c6 141 start_calibration++;
NahuelM 2:57523bb4e9c6 142 }
NahuelM 2:57523bb4e9c6 143 else {
NahuelM 2:57523bb4e9c6 144 //finding min and max of emg0
NahuelM 12:12c162dc8893 145
pbaardwijk 9:1cb2d5ab51e6 146 if (emg_low_passed[0] < min_emg[0]) {
pbaardwijk 9:1cb2d5ab51e6 147 min_emg[0] = emg_low_passed[0];
NahuelM 2:57523bb4e9c6 148 }
pbaardwijk 9:1cb2d5ab51e6 149 else if (emg_low_passed[0] > max_emg[0]) {
pbaardwijk 9:1cb2d5ab51e6 150 max_emg[0] = emg_low_passed[0];
NahuelM 2:57523bb4e9c6 151 }
NahuelM 2:57523bb4e9c6 152
NahuelM 2:57523bb4e9c6 153 //finding min and max of emg1
pbaardwijk 9:1cb2d5ab51e6 154 if (emg_low_passed[1] < min_emg[1]) {
pbaardwijk 9:1cb2d5ab51e6 155 min_emg[1] = emg_low_passed[1];
NahuelM 2:57523bb4e9c6 156 }
pbaardwijk 9:1cb2d5ab51e6 157 else if (emg_low_passed[1] > max_emg[1]) {
pbaardwijk 9:1cb2d5ab51e6 158 max_emg[1] = emg_low_passed[1];
NahuelM 2:57523bb4e9c6 159 }
NahuelM 2:57523bb4e9c6 160
NahuelM 2:57523bb4e9c6 161 //finding min and max of emg2
pbaardwijk 9:1cb2d5ab51e6 162 if (emg_low_passed[2] < min_emg[2]) {
pbaardwijk 9:1cb2d5ab51e6 163 min_emg[2] = emg_low_passed[2];
NahuelM 2:57523bb4e9c6 164 }
pbaardwijk 9:1cb2d5ab51e6 165 else if (emg_low_passed[2] > max_emg[2]) {
pbaardwijk 9:1cb2d5ab51e6 166 max_emg[2] = emg_low_passed[2];
NahuelM 2:57523bb4e9c6 167 }
NahuelM 2:57523bb4e9c6 168 }
NahuelM 2:57523bb4e9c6 169 }
NahuelM 2:57523bb4e9c6 170
NahuelM 2:57523bb4e9c6 171 //calculating input_forces for controller
pbaardwijk 9:1cb2d5ab51e6 172 Norm_EMG_0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]);
pbaardwijk 9:1cb2d5ab51e6 173 Norm_EMG_1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]);
pbaardwijk 9:1cb2d5ab51e6 174 Norm_EMG_2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]);
NahuelM 2:57523bb4e9c6 175
NahuelM 2:57523bb4e9c6 176 //Send scope data
NahuelM 11:c8b6a2b314c3 177 //scope.set(0,Norm_EMG_0);
NahuelM 11:c8b6a2b314c3 178 //scope.set(1,Norm_EMG_1);
NahuelM 11:c8b6a2b314c3 179 //scope.set(2,Norm_EMG_2);
NahuelM 2:57523bb4e9c6 180
NahuelM 2:57523bb4e9c6 181 go_emgSample = false;
NahuelM 2:57523bb4e9c6 182 }
NahuelM 2:57523bb4e9c6 183 }
NahuelM 2:57523bb4e9c6 184