Leon Klute / Mbed 2 deprecated EMG_Controller_6

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_5 by Nahuel Manterola

emg.h@13:0adbf6a5de37, 2016-10-28 (annotated)

Committer:
LeeJon
Date:
Fri Oct 28 11:58:56 2016 +0000
Revision:
13:0adbf6a5de37
Parent:
12:12c162dc8893 
de state machine eerste poging

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
LeeJon 13:0adbf6a5de37 5 enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN, STATE_GRAB , STATE_MOVE};
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;
LeeJon 13:0adbf6a5de37 101
pbaardwijk 6:6cb7c0247560 102 led.write(1);
NahuelM 2:57523bb4e9c6 103 }
NahuelM 2:57523bb4e9c6 104
NahuelM 2:57523bb4e9c6 105 void EMG_filter();
NahuelM 2:57523bb4e9c6 106
NahuelM 2:57523bb4e9c6 107 void EMG_filter() {
NahuelM 2:57523bb4e9c6 108 if(go_emgSample == true){
NahuelM 12:12c162dc8893 109 //read the emg signal
NahuelM 2:57523bb4e9c6 110 emg_sample[0] = emg0.read();
NahuelM 2:57523bb4e9c6 111 emg_sample[1] = emg1.read();
NahuelM 2:57523bb4e9c6 112 emg_sample[2] = emg2.read();
NahuelM 12:12c162dc8893 113 //pc.printf("%f - %f - %f \r\n", emg_sample[0], emg_sample[1], emg_sample[2]);
pbaardwijk 9:1cb2d5ab51e6 114 //filter out the 50Hz components with a notch filter
NahuelM 12:12c162dc8893 115 emg_notch[0] = notch_50_0.step(emg_sample[0]);
NahuelM 12:12c162dc8893 116 emg_notch[1] = notch_50_1.step(emg_sample[1]);
NahuelM 12:12c162dc8893 117 emg_notch[2] = notch_50_2.step(emg_sample[2]);
pbaardwijk 9:1cb2d5ab51e6 118 //high pass the signal (removing motion artifacts and offset)
NahuelM 12:12c162dc8893 119 emg_high_passed[0] = high_pass_0.step(emg_notch[0]);
NahuelM 12:12c162dc8893 120 emg_high_passed[1] = high_pass_1.step(emg_notch[1]);
NahuelM 12:12c162dc8893 121 emg_high_passed[2] = high_pass_2.step(emg_notch[2]);
NahuelM 12:12c162dc8893 122 float emg_fabs[3];
NahuelM 12:12c162dc8893 123 emg_fabs[0] = fabs(emg_high_passed[0]);
NahuelM 12:12c162dc8893 124 emg_fabs[1] = fabs(emg_high_passed[1]);
NahuelM 12:12c162dc8893 125 emg_fabs[2] = fabs(emg_high_passed[2]);
pbaardwijk 9:1cb2d5ab51e6 126 //low pass the rectified emg signal
NahuelM 12:12c162dc8893 127 emg_low_passed[0] = low_pass_0.step(emg_fabs[0]);
NahuelM 12:12c162dc8893 128 emg_low_passed[1] = low_pass_1.step(emg_fabs[1]);
NahuelM 12:12c162dc8893 129 emg_low_passed[2] = low_pass_2.step(emg_fabs[2]);
NahuelM 2:57523bb4e9c6 130
NahuelM 12:12c162dc8893 131 //pc.printf("%f - %f - %f \r\n", emg_low_passed[0], emg_low_passed[1], emg_low_passed[2]);
NahuelM 2:57523bb4e9c6 132 //Calculating min value and max value of emg signal
NahuelM 2:57523bb4e9c6 133 if(state == STATE_CALIBRATION)
NahuelM 2:57523bb4e9c6 134 {
NahuelM 2:57523bb4e9c6 135 if (start_calibration == 0) {
pbaardwijk 9:1cb2d5ab51e6 136 min_emg[0] = emg_low_passed[0];
pbaardwijk 9:1cb2d5ab51e6 137 max_emg[0] = emg_low_passed[0];
pbaardwijk 9:1cb2d5ab51e6 138 min_emg[1] = emg_low_passed[1];
pbaardwijk 9:1cb2d5ab51e6 139 max_emg[1] = emg_low_passed[1];
pbaardwijk 9:1cb2d5ab51e6 140 min_emg[2] = emg_low_passed[2];
pbaardwijk 9:1cb2d5ab51e6 141 max_emg[2] = emg_low_passed[2];
NahuelM 2:57523bb4e9c6 142 start_calibration++;
NahuelM 2:57523bb4e9c6 143 }
NahuelM 2:57523bb4e9c6 144 else {
NahuelM 2:57523bb4e9c6 145 //finding min and max of emg0
NahuelM 12:12c162dc8893 146
pbaardwijk 9:1cb2d5ab51e6 147 if (emg_low_passed[0] < min_emg[0]) {
pbaardwijk 9:1cb2d5ab51e6 148 min_emg[0] = emg_low_passed[0];
NahuelM 2:57523bb4e9c6 149 }
pbaardwijk 9:1cb2d5ab51e6 150 else if (emg_low_passed[0] > max_emg[0]) {
pbaardwijk 9:1cb2d5ab51e6 151 max_emg[0] = emg_low_passed[0];
NahuelM 2:57523bb4e9c6 152 }
NahuelM 2:57523bb4e9c6 153
NahuelM 2:57523bb4e9c6 154 //finding min and max of emg1
pbaardwijk 9:1cb2d5ab51e6 155 if (emg_low_passed[1] < min_emg[1]) {
pbaardwijk 9:1cb2d5ab51e6 156 min_emg[1] = emg_low_passed[1];
NahuelM 2:57523bb4e9c6 157 }
pbaardwijk 9:1cb2d5ab51e6 158 else if (emg_low_passed[1] > max_emg[1]) {
pbaardwijk 9:1cb2d5ab51e6 159 max_emg[1] = emg_low_passed[1];
NahuelM 2:57523bb4e9c6 160 }
NahuelM 2:57523bb4e9c6 161
NahuelM 2:57523bb4e9c6 162 //finding min and max of emg2
pbaardwijk 9:1cb2d5ab51e6 163 if (emg_low_passed[2] < min_emg[2]) {
pbaardwijk 9:1cb2d5ab51e6 164 min_emg[2] = emg_low_passed[2];
NahuelM 2:57523bb4e9c6 165 }
pbaardwijk 9:1cb2d5ab51e6 166 else if (emg_low_passed[2] > max_emg[2]) {
pbaardwijk 9:1cb2d5ab51e6 167 max_emg[2] = emg_low_passed[2];
NahuelM 2:57523bb4e9c6 168 }
NahuelM 2:57523bb4e9c6 169 }
NahuelM 2:57523bb4e9c6 170 }
NahuelM 2:57523bb4e9c6 171
NahuelM 2:57523bb4e9c6 172 //calculating input_forces for controller
pbaardwijk 9:1cb2d5ab51e6 173 Norm_EMG_0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]);
pbaardwijk 9:1cb2d5ab51e6 174 Norm_EMG_1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]);
pbaardwijk 9:1cb2d5ab51e6 175 Norm_EMG_2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]);
NahuelM 2:57523bb4e9c6 176
NahuelM 2:57523bb4e9c6 177 //Send scope data
NahuelM 11:c8b6a2b314c3 178 //scope.set(0,Norm_EMG_0);
NahuelM 11:c8b6a2b314c3 179 //scope.set(1,Norm_EMG_1);
NahuelM 11:c8b6a2b314c3 180 //scope.set(2,Norm_EMG_2);
NahuelM 2:57523bb4e9c6 181
NahuelM 2:57523bb4e9c6 182 go_emgSample = false;
NahuelM 2:57523bb4e9c6 183 }
NahuelM 2:57523bb4e9c6 184 }
NahuelM 2:57523bb4e9c6 185