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@10:25d7600d1e38, 2016-10-27 (annotated)

Committer:
pbaardwijk
Date:
Thu Oct 27 11:50:16 2016 +0000
Revision:
10:25d7600d1e38
Parent:
9:1cb2d5ab51e6 
Controller with new EMG filter

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
pbaardwijk 10:25d7600d1e38 11 HIDScope scope(3);
NahuelM 2:57523bb4e9c6 12
NahuelM 2:57523bb4e9c6 13 //Notch filter
NahuelM 2:57523bb4e9c6 14 BiQuadChain notch_50;
pbaardwijk 9:1cb2d5ab51e6 15 BiQuad bq3( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
pbaardwijk 9:1cb2d5ab51e6 16 BiQuad bq4( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
pbaardwijk 9:1cb2d5ab51e6 17 BiQuad bq5( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
NahuelM 2:57523bb4e9c6 18
NahuelM 2:57523bb4e9c6 19 //High pass filter
NahuelM 2:57523bb4e9c6 20 BiQuadChain high_pass;
pbaardwijk 9:1cb2d5ab51e6 21 BiQuad bq6( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
pbaardwijk 9:1cb2d5ab51e6 22 BiQuad bq7( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
pbaardwijk 9:1cb2d5ab51e6 23 //BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620);
NahuelM 2:57523bb4e9c6 24
pbaardwijk 9:1cb2d5ab51e6 25 //Low pass filter
pbaardwijk 9:1cb2d5ab51e6 26 BiQuadChain low_pass;
pbaardwijk 9:1cb2d5ab51e6 27 BiQuad bq9( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
pbaardwijk 9:1cb2d5ab51e6 28 BiQuad bq10( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
pbaardwijk 9:1cb2d5ab51e6 29 BiQuad bq11( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
NahuelM 2:57523bb4e9c6 30 //Ticker
NahuelM 2:57523bb4e9c6 31 Ticker emgSampleTicker;
NahuelM 2:57523bb4e9c6 32
NahuelM 2:57523bb4e9c6 33 //Timeout to change state after 5 seconds
NahuelM 2:57523bb4e9c6 34 Timeout change_state;
NahuelM 2:57523bb4e9c6 35
pbaardwijk 9:1cb2d5ab51e6 36 //Timeout to change state after 15 seconds
NahuelM 2:57523bb4e9c6 37 Timeout change_state2;
NahuelM 2:57523bb4e9c6 38
pbaardwijk 9:1cb2d5ab51e6 39 //LED
pbaardwijk 6:6cb7c0247560 40 DigitalOut led(LED_RED);
pbaardwijk 6:6cb7c0247560 41
NahuelM 2:57523bb4e9c6 42 //Emg input
NahuelM 2:57523bb4e9c6 43 AnalogIn emg0( A0 );
NahuelM 2:57523bb4e9c6 44 AnalogIn emg1( A1 );
NahuelM 2:57523bb4e9c6 45 AnalogIn emg2( A2 );
NahuelM 2:57523bb4e9c6 46
pbaardwijk 9:1cb2d5ab51e6 47 //Go flag for emg sample
NahuelM 2:57523bb4e9c6 48 bool go_emgSample;
pbaardwijk 9:1cb2d5ab51e6 49
pbaardwijk 9:1cb2d5ab51e6 50 //Variables for intermediate filter values
NahuelM 2:57523bb4e9c6 51 double emg_sample[3];
pbaardwijk 9:1cb2d5ab51e6 52 double emg_low_passed_200[3];
NahuelM 2:57523bb4e9c6 53 double emg_notch[3];
NahuelM 2:57523bb4e9c6 54 double emg_high_passed[3];
pbaardwijk 9:1cb2d5ab51e6 55 double emg_low_passed[3];
NahuelM 2:57523bb4e9c6 56 double min_emg[3];
NahuelM 2:57523bb4e9c6 57 double max_emg[3];
NahuelM 2:57523bb4e9c6 58
pbaardwijk 9:1cb2d5ab51e6 59 //Calculating normalized outputs
NahuelM 3:1d43dd4f37eb 60 double Norm_EMG_0;
NahuelM 3:1d43dd4f37eb 61 double Norm_EMG_1;
NahuelM 3:1d43dd4f37eb 62 double Norm_EMG_2;
NahuelM 2:57523bb4e9c6 63
NahuelM 2:57523bb4e9c6 64 //count for emg min max
NahuelM 2:57523bb4e9c6 65 int start_calibration = 0;
NahuelM 2:57523bb4e9c6 66
NahuelM 2:57523bb4e9c6 67 void emgSample() {
NahuelM 2:57523bb4e9c6 68 go_emgSample = true;
NahuelM 2:57523bb4e9c6 69 }
NahuelM 2:57523bb4e9c6 70
NahuelM 2:57523bb4e9c6 71 void calibrate() {
pbaardwijk 9:1cb2d5ab51e6 72 state = STATE_CALIBRATION;
pbaardwijk 9:1cb2d5ab51e6 73 led.write(0);
NahuelM 2:57523bb4e9c6 74 }
NahuelM 2:57523bb4e9c6 75
NahuelM 2:57523bb4e9c6 76 void run() {
NahuelM 2:57523bb4e9c6 77 state = STATE_RUN;
pbaardwijk 6:6cb7c0247560 78 led.write(1);
NahuelM 2:57523bb4e9c6 79 }
NahuelM 2:57523bb4e9c6 80
NahuelM 2:57523bb4e9c6 81 void EMG_filter();
NahuelM 2:57523bb4e9c6 82
NahuelM 2:57523bb4e9c6 83 void EMG_filter() {
NahuelM 2:57523bb4e9c6 84 if(go_emgSample == true){
NahuelM 2:57523bb4e9c6 85 //read the emg signal
NahuelM 2:57523bb4e9c6 86 emg_sample[0] = emg0.read();
NahuelM 2:57523bb4e9c6 87 emg_sample[1] = emg1.read();
NahuelM 2:57523bb4e9c6 88 emg_sample[2] = emg2.read();
NahuelM 2:57523bb4e9c6 89
pbaardwijk 9:1cb2d5ab51e6 90 //filter out the 50Hz components with a notch filter
pbaardwijk 9:1cb2d5ab51e6 91 emg_notch[0] = notch_50.step(emg_sample[0]);
NahuelM 2:57523bb4e9c6 92
pbaardwijk 9:1cb2d5ab51e6 93 //high pass the signal (removing motion artifacts and offset)
pbaardwijk 9:1cb2d5ab51e6 94 emg_high_passed[0] = high_pass.step(emg_notch[0]);
pbaardwijk 9:1cb2d5ab51e6 95
pbaardwijk 9:1cb2d5ab51e6 96 //low pass the rectified emg signal
pbaardwijk 9:1cb2d5ab51e6 97 emg_low_passed[0] = low_pass.step(fabs(emg_high_passed[0]));
NahuelM 2:57523bb4e9c6 98
NahuelM 2:57523bb4e9c6 99
NahuelM 2:57523bb4e9c6 100 //Calculating min value and max value of emg signal
NahuelM 2:57523bb4e9c6 101 if(state == STATE_CALIBRATION)
NahuelM 2:57523bb4e9c6 102 {
NahuelM 2:57523bb4e9c6 103 if (start_calibration == 0) {
pbaardwijk 9:1cb2d5ab51e6 104 min_emg[0] = emg_low_passed[0];
pbaardwijk 9:1cb2d5ab51e6 105 max_emg[0] = emg_low_passed[0];
pbaardwijk 9:1cb2d5ab51e6 106 min_emg[1] = emg_low_passed[1];
pbaardwijk 9:1cb2d5ab51e6 107 max_emg[1] = emg_low_passed[1];
pbaardwijk 9:1cb2d5ab51e6 108 min_emg[2] = emg_low_passed[2];
pbaardwijk 9:1cb2d5ab51e6 109 max_emg[2] = emg_low_passed[2];
NahuelM 2:57523bb4e9c6 110 start_calibration++;
NahuelM 2:57523bb4e9c6 111 }
NahuelM 2:57523bb4e9c6 112 else {
NahuelM 2:57523bb4e9c6 113 //finding min and max of emg0
pbaardwijk 9:1cb2d5ab51e6 114 if (emg_low_passed[0] < min_emg[0]) {
pbaardwijk 9:1cb2d5ab51e6 115 min_emg[0] = emg_low_passed[0];
NahuelM 2:57523bb4e9c6 116 }
pbaardwijk 9:1cb2d5ab51e6 117 else if (emg_low_passed[0] > max_emg[0]) {
pbaardwijk 9:1cb2d5ab51e6 118 max_emg[0] = emg_low_passed[0];
NahuelM 2:57523bb4e9c6 119 }
NahuelM 2:57523bb4e9c6 120
NahuelM 2:57523bb4e9c6 121 //finding min and max of emg1
pbaardwijk 9:1cb2d5ab51e6 122 if (emg_low_passed[1] < min_emg[1]) {
pbaardwijk 9:1cb2d5ab51e6 123 min_emg[1] = emg_low_passed[1];
NahuelM 2:57523bb4e9c6 124 }
pbaardwijk 9:1cb2d5ab51e6 125 else if (emg_low_passed[1] > max_emg[1]) {
pbaardwijk 9:1cb2d5ab51e6 126 max_emg[1] = emg_low_passed[1];
NahuelM 2:57523bb4e9c6 127 }
NahuelM 2:57523bb4e9c6 128
NahuelM 2:57523bb4e9c6 129 //finding min and max of emg2
pbaardwijk 9:1cb2d5ab51e6 130 if (emg_low_passed[2] < min_emg[2]) {
pbaardwijk 9:1cb2d5ab51e6 131 min_emg[2] = emg_low_passed[2];
NahuelM 2:57523bb4e9c6 132 }
pbaardwijk 9:1cb2d5ab51e6 133 else if (emg_low_passed[2] > max_emg[2]) {
pbaardwijk 9:1cb2d5ab51e6 134 max_emg[2] = emg_low_passed[2];
NahuelM 2:57523bb4e9c6 135 }
NahuelM 2:57523bb4e9c6 136 }
NahuelM 2:57523bb4e9c6 137 }
NahuelM 2:57523bb4e9c6 138
NahuelM 2:57523bb4e9c6 139 //calculating input_forces for controller
pbaardwijk 9:1cb2d5ab51e6 140 Norm_EMG_0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]);
pbaardwijk 9:1cb2d5ab51e6 141 Norm_EMG_1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]);
pbaardwijk 9:1cb2d5ab51e6 142 Norm_EMG_2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]);
NahuelM 2:57523bb4e9c6 143
NahuelM 2:57523bb4e9c6 144 //Send scope data
pbaardwijk 10:25d7600d1e38 145 scope.set(0,Norm_EMG_0);
pbaardwijk 10:25d7600d1e38 146 scope.set(1,Norm_EMG_1);
pbaardwijk 10:25d7600d1e38 147 scope.set(2,Norm_EMG_2);
NahuelM 2:57523bb4e9c6 148
NahuelM 2:57523bb4e9c6 149 go_emgSample = false;
NahuelM 2:57523bb4e9c6 150 }
NahuelM 2:57523bb4e9c6 151 }
NahuelM 2:57523bb4e9c6 152