Anaïs Chaumeil / Mbed 2 deprecated EMG_processing_biorobotics_group19

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:   mbed HIDScope FXOS8700Q

main.cpp@1:0b3280b1795e, 2019-10-14 (annotated)

Committer:
AnaisChaumeil
Date:
Mon Oct 14 16:43:28 2019 +0000
Revision:
1:0b3280b1795e
Parent:
0:971166224b2d
Child:
2:c177a3e9708e
code presented on monday 14th

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AnaisChaumeil 0:971166224b2d 1 #include "mbed.h"
AnaisChaumeil 0:971166224b2d 2 #include "math.h"
AnaisChaumeil 0:971166224b2d 3
AnaisChaumeil 0:971166224b2d 4 // CODE THE PART WHERE WE PUT THE SIGNAL INTO RAW_SIGNAL
AnaisChaumeil 0:971166224b2d 5
AnaisChaumeil 0:971166224b2d 6 // Defining objects
AnaisChaumeil 0:971166224b2d 7
AnaisChaumeil 0:971166224b2d 8 AnalogIn emg0( A0 ); // for movement up/down, named A
AnaisChaumeil 0:971166224b2d 9 AnalogIn emg1( A1 );
AnaisChaumeil 0:971166224b2d 10
AnaisChaumeil 0:971166224b2d 11 AnalogIn emg2( A2 ); // for movement forward/backward, named B
AnaisChaumeil 0:971166224b2d 12 AnalogIn emg3( A3 );
AnaisChaumeil 0:971166224b2d 13
AnaisChaumeil 1:0b3280b1795e 14 //int mark=0; // allows us to know where we are exactly in the sampling process
AnaisChaumeil 0:971166224b2d 15
AnaisChaumeil 0:971166224b2d 16 const double pi=3.14;
AnaisChaumeil 0:971166224b2d 17
AnaisChaumeil 0:971166224b2d 18 // define two functions that will be attached to the tickers
AnaisChaumeil 0:971166224b2d 19 Ticker sampling; // ticker for the sampling of the signal, every 0.002s a value is sampled
AnaisChaumeil 0:971166224b2d 20 Ticker processing; //ticker that defines the signal processed, every second a signal is defined
AnaisChaumeil 0:971166224b2d 21
AnaisChaumeil 0:971166224b2d 22 double raw_signal_0[500]; // has the values measured via the electrodes
AnaisChaumeil 0:971166224b2d 23 double raw_signal_1[500];
AnaisChaumeil 0:971166224b2d 24 double raw_signal_2[500];
AnaisChaumeil 0:971166224b2d 25 double raw_signal_3[500];
AnaisChaumeil 0:971166224b2d 26
AnaisChaumeil 0:971166224b2d 27 double filt0[500]; // raw_emg_1 filtered by high pass filter
AnaisChaumeil 0:971166224b2d 28 double filt1[500];
AnaisChaumeil 0:971166224b2d 29 double filt2[500];
AnaisChaumeil 0:971166224b2d 30 double filt3[500];
AnaisChaumeil 0:971166224b2d 31
AnaisChaumeil 0:971166224b2d 32 double filt_filt0[500]; // filt1 filtered by low pass filter
AnaisChaumeil 0:971166224b2d 33 double filt_filt1[500];
AnaisChaumeil 0:971166224b2d 34 double filt_filt2[500];
AnaisChaumeil 0:971166224b2d 35 double filt_filt3[500];
AnaisChaumeil 0:971166224b2d 36
AnaisChaumeil 0:971166224b2d 37 double emgA[500]; //difference between the two channels
AnaisChaumeil 0:971166224b2d 38 double emgB[500];
AnaisChaumeil 0:971166224b2d 39
AnaisChaumeil 0:971166224b2d 40
AnaisChaumeil 0:971166224b2d 41 double emg_absA[500]; // absolute value applied to emg
AnaisChaumeil 0:971166224b2d 42 double emg_absB[500];
AnaisChaumeil 0:971166224b2d 43
AnaisChaumeil 0:971166224b2d 44 double emg_maA[500]; // moving average applied to emg_abs
AnaisChaumeil 0:971166224b2d 45 double emg_maB[500];
AnaisChaumeil 0:971166224b2d 46
AnaisChaumeil 0:971166224b2d 47 double emg_rmsA[500]; // rms applied to emg_ma
AnaisChaumeil 0:971166224b2d 48 double emg_rmsB[500];
AnaisChaumeil 0:971166224b2d 49
AnaisChaumeil 0:971166224b2d 50 double emg_normA[500]; // normalization of emg_rms
AnaisChaumeil 0:971166224b2d 51 double emg_normB[500];
AnaisChaumeil 0:971166224b2d 52
AnaisChaumeil 1:0b3280b1795e 53 bool modeA= false; // running motor at intermediate speed, derived from threshold
AnaisChaumeil 1:0b3280b1795e 54 bool modeB= false;
AnaisChaumeil 0:971166224b2d 55
AnaisChaumeil 1:0b3280b1795e 56 bool speedA= false; // running motor at high speed
AnaisChaumeil 1:0b3280b1795e 57 bool speedB= false;
AnaisChaumeil 0:971166224b2d 58
AnaisChaumeil 0:971166224b2d 59 double T=0.002; // time between two samples
AnaisChaumeil 0:971166224b2d 60
AnaisChaumeil 0:971166224b2d 61
AnaisChaumeil 0:971166224b2d 62 void sample()
AnaisChaumeil 0:971166224b2d 63 {
AnaisChaumeil 0:971166224b2d 64 raw_signal_0[mark]=emg0.read();
AnaisChaumeil 0:971166224b2d 65 raw_signal_1[mark]=emg1.read();
AnaisChaumeil 0:971166224b2d 66 raw_signal_2[mark]=emg2.read();
AnaisChaumeil 0:971166224b2d 67 raw_signal_3[mark]=emg3.read();
AnaisChaumeil 0:971166224b2d 68 mark++;
AnaisChaumeil 0:971166224b2d 69 }
AnaisChaumeil 0:971166224b2d 70
AnaisChaumeil 0:971166224b2d 71
AnaisChaumeil 0:971166224b2d 72 // BIG FUNCTION THAT INCLUDES EVERYTHING
AnaisChaumeil 0:971166224b2d 73
AnaisChaumeil 0:971166224b2d 74 void processing_signals()
AnaisChaumeil 0:971166224b2d 75 {
AnaisChaumeil 0:971166224b2d 76 mark=0;
AnaisChaumeil 0:971166224b2d 77
AnaisChaumeil 0:971166224b2d 78 // LOWPASS FILTER
AnaisChaumeil 0:971166224b2d 79 double wclp=300*2*pi; // cutoff omega for low pass filter
AnaisChaumeil 0:971166224b2d 80
AnaisChaumeil 0:971166224b2d 81 // initialization of the filt arrays
AnaisChaumeil 0:971166224b2d 82 filt0[0]=(raw_signal_0[0]+raw_signal_0[1])/2; // we take the mean of the two first values to initialize the signal
AnaisChaumeil 0:971166224b2d 83 filt1[0]=(raw_signal_1[0]+raw_signal_1[1])/2;
AnaisChaumeil 0:971166224b2d 84 filt2[0]=(raw_signal_2[0]+raw_signal_2[1])/2;
AnaisChaumeil 0:971166224b2d 85 filt3[0]=(raw_signal_3[0]+raw_signal_3[1])/2;
AnaisChaumeil 0:971166224b2d 86
AnaisChaumeil 0:971166224b2d 87 // initialization of the parameters
AnaisChaumeil 0:971166224b2d 88 double b0=(wclp*T)/(wclp*T+2);
AnaisChaumeil 0:971166224b2d 89 double b1=(wclp*T)/(wclp*T+2);
AnaisChaumeil 0:971166224b2d 90 double a1=(wclp*T-2)/(wclp*T+2);
AnaisChaumeil 0:971166224b2d 91 int a;
AnaisChaumeil 0:971166224b2d 92 for (a=1;a<500;a++) // the for loop starts at i=1 because we have already set values for i=0
AnaisChaumeil 0:971166224b2d 93 {
AnaisChaumeil 0:971166224b2d 94 filt0[a]=b0*raw_signal_0[a]+b1*raw_signal_0[a-1]-a1*filt0[a-1];
AnaisChaumeil 0:971166224b2d 95 filt1[a]=b0*raw_signal_1[a]+b1*raw_signal_1[a-1]-a1*filt1[a-1];
AnaisChaumeil 0:971166224b2d 96 filt2[a]=b0*raw_signal_2[a]+b1*raw_signal_2[a-1]-a1*filt2[a-1];
AnaisChaumeil 0:971166224b2d 97 filt3[a]=b0*raw_signal_3[a]+b1*raw_signal_3[a-1]-a1*filt3[a-1];
AnaisChaumeil 0:971166224b2d 98 }
AnaisChaumeil 0:971166224b2d 99
AnaisChaumeil 0:971166224b2d 100 // HIGH PASS FILTER
AnaisChaumeil 0:971166224b2d 101 double wchp=10*2*pi;
AnaisChaumeil 0:971166224b2d 102
AnaisChaumeil 0:971166224b2d 103 // initialization by the mean of the two first values
AnaisChaumeil 0:971166224b2d 104 filt_filt0[0]=(filt0[0]+filt0[1])/2;
AnaisChaumeil 0:971166224b2d 105 filt_filt1[0]=(filt1[0]+filt1[1])/2;
AnaisChaumeil 0:971166224b2d 106 filt_filt2[0]=(filt2[0]+filt2[1])/2;
AnaisChaumeil 0:971166224b2d 107 filt_filt4[0]=(filt4[0]+filt4[1])/2;
AnaisChaumeil 0:971166224b2d 108
AnaisChaumeil 0:971166224b2d 109 // initialization of the parameters
AnaisChaumeil 0:971166224b2d 110 double d0=2/(wchp*T+2);
AnaisChaumeil 0:971166224b2d 111 double d1=2/(wchp*T+2);
AnaisChaumeil 0:971166224b2d 112 double c1=(wchp*T-2)/(wchp*T+2);
AnaisChaumeil 0:971166224b2d 113 int b;
AnaisChaumeil 0:971166224b2d 114 for (b=1;b<500;b++)
AnaisChaumeil 0:971166224b2d 115 {
AnaisChaumeil 0:971166224b2d 116 filt_filt0[b]=d0*filt0[b]+d1*filt0[b-1]-c1*filt_filt0[b-1];
AnaisChaumeil 0:971166224b2d 117 filt_filt1[b]=d0*filt1[b]+d1*filt1[b-1]-c1*filt_filt1[b-1];
AnaisChaumeil 0:971166224b2d 118 filt_filt2[b]=d0*filt2[b]+d1*filt2[b-1]-c1*filt_filt2[b-1];
AnaisChaumeil 0:971166224b2d 119 filt_filt3[b]=d0*filt3[b]+d1*filt3[b-1]-c1*filt_filt3[b-1];
AnaisChaumeil 0:971166224b2d 120 }
AnaisChaumeil 0:971166224b2d 121
AnaisChaumeil 0:971166224b2d 122 // DIFFERENCE OF THE SIGNALS AND ABSOLUTE VALUE
AnaisChaumeil 0:971166224b2d 123 int c;
AnaisChaumeil 0:971166224b2d 124 for (c=0;c<500;c++)
AnaisChaumeil 0:971166224b2d 125 {
AnaisChaumeil 0:971166224b2d 126 emgA[c]=filt_filt0[c]-filt_filt1[c];
AnaisChaumeil 0:971166224b2d 127 emg_absA[c]=fabs(emgA[c]);
AnaisChaumeil 0:971166224b2d 128 emgB[c]=filt_filt2[c]-filt_filt3[c];
AnaisChaumeil 0:971166224b2d 129 emg_absB[c]=fabs(emgB[c]);
AnaisChaumeil 0:971166224b2d 130 }
AnaisChaumeil 0:971166224b2d 131
AnaisChaumeil 0:971166224b2d 132 // MOVING AVERAGE
AnaisChaumeil 0:971166224b2d 133 int i;
AnaisChaumeil 0:971166224b2d 134 for(i=5;i<500;i++)
AnaisChaumeil 0:971166224b2d 135 {
AnaisChaumeil 0:971166224b2d 136 double val4=(emg_absA[i-4]+emg_absA[i-3]+emg_absA[i-2]+emg_absA[i-1]+emg_absA[i])/5;
AnaisChaumeil 0:971166224b2d 137 double val3=(emg_absA[i-3]+emg_absA[i-2]+emg_absA[i-1]+emg_absA[i])/4;
AnaisChaumeil 0:971166224b2d 138 double val2=(emg_absA[i-2]+emg_absA[i-1]+emg_absA[i])/3;
AnaisChaumeil 0:971166224b2d 139 double val1=(emg_absA[i-1]+emg_absA[i])/2;
AnaisChaumeil 0:971166224b2d 140 double val0=emg_absA[i];
AnaisChaumeil 0:971166224b2d 141 emg_maA=(val0+val1+val2+val3+val4)/5;
AnaisChaumeil 0:971166224b2d 142 }
AnaisChaumeil 0:971166224b2d 143 int ii;
AnaisChaumeil 0:971166224b2d 144 for(ii=5;ii<500;ii++) // we can reuse val4,3,2,1,0 because they are local variables
AnaisChaumeil 0:971166224b2d 145 {
AnaisChaumeil 0:971166224b2d 146 double val4=(emg_absB[ii-4]+emg_absB[ii-3]+emg_absB[ii-2]+emg_absB[ii-1]+emg_absB[ii])/5;
AnaisChaumeil 0:971166224b2d 147 double val3=(emg_absB[ii-3]+emg_absB[ii-2]+emg_absB[ii-1]+emg_absB[ii])/4;
AnaisChaumeil 0:971166224b2d 148 double val2=(emg_absB[ii-2]+emg_absB[ii-1]+emg_absB[ii])/3;
AnaisChaumeil 0:971166224b2d 149 double val1=(emg_absB[ii-1]+emg_absB[ii])/2;
AnaisChaumeil 0:971166224b2d 150 double val0=emg_absB[ii];
AnaisChaumeil 0:971166224b2d 151 emg_maB=(val0+val1+val2+val3+val4)/5;
AnaisChaumeil 0:971166224b2d 152 }
AnaisChaumeil 0:971166224b2d 153
AnaisChaumeil 0:971166224b2d 154 // ROOT MEAN SQUARE
AnaisChaumeil 0:971166224b2d 155
AnaisChaumeil 0:971166224b2d 156 int d;
AnaisChaumeil 0:971166224b2d 157 double sumA, sumB;
AnaisChaumeil 0:971166224b2d 158 for (d=5;d<500;d++)
AnaisChaumeil 0:971166224b2d 159 {
AnaisChaumeil 0:971166224b2d 160 sumA=emg_maA[d-4]^2+emg_maA[d-3]^2+emg_maA[d-2]^2+emg_maA[d-1]^2+emg_maA[d]^2;
AnaisChaumeil 0:971166224b2d 161 emg_rmsA[d]=sqrt(sumA/5);
AnaisChaumeil 0:971166224b2d 162 sumB=emg_maB[d-4]^2+emg_maB[d-3]^2+emg_maB[d-2]^2+emg_maB[d-1]^2+emg_maB[d]^2;
AnaisChaumeil 0:971166224b2d 163 emg_rmsB[d]=sqrt(sumB/5);
AnaisChaumeil 0:971166224b2d 164 }
AnaisChaumeil 0:971166224b2d 165
AnaisChaumeil 0:971166224b2d 166
AnaisChaumeil 0:971166224b2d 167 emg_rmsA[0]=emg_absA[0];
AnaisChaumeil 0:971166224b2d 168 emg_rmsA[1]=emg_absA[1];
AnaisChaumeil 0:971166224b2d 169 emg_rmsA[2]=emg_absA[2];
AnaisChaumeil 0:971166224b2d 170 emg_rmsA[3]=emg_absA[3];
AnaisChaumeil 0:971166224b2d 171 emg_rmsA[4]=emg_absA[4];
AnaisChaumeil 0:971166224b2d 172
AnaisChaumeil 0:971166224b2d 173 emg_rmsB[0]=emg_absB[0];
AnaisChaumeil 0:971166224b2d 174 emg_rmsB[1]=emg_absB[1];
AnaisChaumeil 0:971166224b2d 175 emg_rmsB[2]=emg_absB[2];
AnaisChaumeil 0:971166224b2d 176 emg_rmsB[3]=emg_absB[3];
AnaisChaumeil 0:971166224b2d 177 emg_rmsB[4]=emg_absB[4];
AnaisChaumeil 0:971166224b2d 178
AnaisChaumeil 0:971166224b2d 179 // NORMALIZATION
AnaisChaumeil 0:971166224b2d 180
AnaisChaumeil 0:971166224b2d 181 int e; // computes the maximum and the minimum of the array
AnaisChaumeil 0:971166224b2d 182 double MEANA=(emg_rmsA[250]+emg_rmsA[251])/2; // random points mean
AnaisChaumeil 0:971166224b2d 183 double MAXA=MEANA+1;
AnaisChaumeil 0:971166224b2d 184 double MINA=MEANA-1;
AnaisChaumeil 0:971166224b2d 185 for (e=0; e<500;e++)
AnaisChaumeil 0:971166224b2d 186 {
AnaisChaumeil 0:971166224b2d 187 if (emg_rmsA[e]>MAXA)
AnaisChaumeil 0:971166224b2d 188 {
AnaisChaumeil 0:971166224b2d 189 MAXA=emg_rmsA[e];
AnaisChaumeil 0:971166224b2d 190 }
AnaisChaumeil 0:971166224b2d 191 if (emg_rmsA[e]<MINA)
AnaisChaumeil 0:971166224b2d 192 {
AnaisChaumeil 0:971166224b2d 193 MINA=emg_rmsA[e];
AnaisChaumeil 0:971166224b2d 194 }
AnaisChaumeil 0:971166224b2d 195 }
AnaisChaumeil 0:971166224b2d 196
AnaisChaumeil 0:971166224b2d 197 int f;
AnaisChaumeil 0:971166224b2d 198 double MEANB=(emg_rmsB[250]+emg_rmsB[251])/2; // random points mean
AnaisChaumeil 0:971166224b2d 199 double MAXB=MEANB+1;
AnaisChaumeil 0:971166224b2d 200 double MINB=MEANB-1;
AnaisChaumeil 0:971166224b2d 201 for (f=0; f<500; f++)
AnaisChaumeil 0:971166224b2d 202 {
AnaisChaumeil 0:971166224b2d 203 if (emg_rmsB[f]>MAXB)
AnaisChaumeil 0:971166224b2d 204 {
AnaisChaumeil 0:971166224b2d 205 MAXB=emg_rmsB[f];
AnaisChaumeil 0:971166224b2d 206 }
AnaisChaumeil 0:971166224b2d 207 if (emg_rmsB[e]<MINB)
AnaisChaumeil 0:971166224b2d 208 {
AnaisChaumeil 0:971166224b2d 209 MINB=emg_rmsB[f];
AnaisChaumeil 0:971166224b2d 210 }
AnaisChaumeil 0:971166224b2d 211 }
AnaisChaumeil 0:971166224b2d 212
AnaisChaumeil 0:971166224b2d 213 int j;
AnaisChaumeil 0:971166224b2d 214 for (j=0;j<500;j++)
AnaisChaumeil 0:971166224b2d 215 {
AnaisChaumeil 0:971166224b2d 216 emg_normA[j]=(emg_rmsA[j]-MINA)/(MAXA-MINA);
AnaisChaumeil 0:971166224b2d 217 emg_normB[j]=(emg_rmsB[j]-MINB)/(MAXB-MINB);
AnaisChaumeil 0:971166224b2d 218 }
AnaisChaumeil 0:971166224b2d 219
AnaisChaumeil 0:971166224b2d 220 // compute some parameter on the array (average) to set the boolean to true or false by thresholding => parameter to be determined by matlab
AnaisChaumeil 0:971166224b2d 221 double meanA, meanB;
AnaisChaumeil 0:971166224b2d 222 double sumA, sumB;
AnaisChaumeil 0:971166224b2d 223 int p;
AnaisChaumeil 0:971166224b2d 224 for (p=0;p<500;p++)
AnaisChaumeil 0:971166224b2d 225 {
AnaisChaumeil 0:971166224b2d 226 sumA=sumA+emg_normA[p];
AnaisChaumeil 0:971166224b2d 227 sumB=sumB+emg_normB[p];
AnaisChaumeil 0:971166224b2d 228 }
AnaisChaumeil 0:971166224b2d 229 meanA=sumA/500;
AnaisChaumeil 0:971166224b2d 230 meanB=sumB/500;
AnaisChaumeil 0:971166224b2d 231
AnaisChaumeil 0:971166224b2d 232 double threshold=0.5; // threshold for setting the motor yes or no
AnaisChaumeil 0:971166224b2d 233 double threshold_speed= 0.7; // threshold for if we go full speed or no
AnaisChaumeil 0:971166224b2d 234 // THESE VALUES CAN BE CHANGED IF NEEDED
AnaisChaumeil 0:971166224b2d 235
AnaisChaumeil 0:971166224b2d 236 if (meanA>threshold)
AnaisChaumeil 0:971166224b2d 237 {
AnaisChaumeil 1:0b3280b1795e 238 modeA=true;
AnaisChaumeil 0:971166224b2d 239 }
AnaisChaumeil 0:971166224b2d 240
AnaisChaumeil 0:971166224b2d 241 if (meanB>threshold)
AnaisChaumeil 0:971166224b2d 242 {
AnaisChaumeil 1:0b3280b1795e 243 modeB=true;
AnaisChaumeil 0:971166224b2d 244 }
AnaisChaumeil 0:971166224b2d 245 if (meanA>threshold_speed)
AnaisChaumeil 0:971166224b2d 246 {
AnaisChaumeil 1:0b3280b1795e 247 speedA=true;
AnaisChaumeil 0:971166224b2d 248 }
AnaisChaumeil 0:971166224b2d 249
AnaisChaumeil 0:971166224b2d 250 if (meanB>threshold_speed)
AnaisChaumeil 0:971166224b2d 251 {
AnaisChaumeil 1:0b3280b1795e 252 speedB=true;
AnaisChaumeil 0:971166224b2d 253 }
AnaisChaumeil 0:971166224b2d 254 }
AnaisChaumeil 0:971166224b2d 255
AnaisChaumeil 0:971166224b2d 256 int main()
AnaisChaumeil 0:971166224b2d 257 {
AnaisChaumeil 0:971166224b2d 258 sampling.attach(&sample, 0.002);
AnaisChaumeil 0:971166224b2d 259 processing.attach(&processing_signals, 1.0);
AnaisChaumeil 0:971166224b2d 260
AnaisChaumeil 0:971166224b2d 261 while (true)
AnaisChaumeil 0:971166224b2d 262 {
AnaisChaumeil 0:971166224b2d 263
AnaisChaumeil 0:971166224b2d 264 }
AnaisChaumeil 0:971166224b2d 265 return 0;
AnaisChaumeil 0:971166224b2d 266 }