BMT M9 Groep01 / Mbed 2 deprecated test_emg

emg

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed TouchButton

Fork of test by BMT M9 Groep01

main.cpp@16:e997a6fd802a, 2014-10-24 (annotated)

Committer:
Tanja2211
Date:
Fri Oct 24 09:48:05 2014 +0000
Revision:
16:e997a6fd802a
Parent:
15:e779bfbeb8ea
Child:
17:5fd768d0504f
filters

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1340735 0:abe0bc5c43b7 1 #include "mbed.h"
s1340735 0:abe0bc5c43b7 2 #include "HIDScope.h"
s1340735 0:abe0bc5c43b7 3 #include "MODSERIAL.h"
s1340735 0:abe0bc5c43b7 4 #include "arm_math.h"
s1340735 0:abe0bc5c43b7 5
s1340735 2:a86b09b00008 6 MODSERIAL pc(USBTX,USBRX);
s1340735 2:a86b09b00008 7
Tanja2211 9:a1890454e5a7 8 HIDScope scope(4);
s1340735 0:abe0bc5c43b7 9
Tanja2211 4:231bb8f5bc81 10 AnalogIn emgB(PTB1);//biceps
Tanja2211 4:231bb8f5bc81 11 AnalogIn emgT(PTB2); // tricep
s1340735 0:abe0bc5c43b7 12
s1340735 0:abe0bc5c43b7 13 //*** OBJECTS ***
s1340735 2:a86b09b00008 14 //bicep
Tanja2211 9:a1890454e5a7 15 uint16_t emg_valueB;
Tanja2211 9:a1890454e5a7 16 float emg_value_f32B;
s1340735 2:a86b09b00008 17 float filtered_emgB;
s1340735 0:abe0bc5c43b7 18 float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
s1340735 0:abe0bc5c43b7 19 int yB1, yB2, yB3;
Tanja2211 15:e779bfbeb8ea 20 float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11, B12, B13, B14, B15, B16, B17, B18, B19, B20, B21, B22, B23, B24, B25, B26, B27, B28, B29,MOVAVG_B; //moving average objects
s1340735 1:6a8b45298e54 21 int snelheidsstand;
s1340735 2:a86b09b00008 22 //tricep
Tanja2211 9:a1890454e5a7 23 uint16_t emg_valueT;
Tanja2211 9:a1890454e5a7 24 float emg_value_f32T;
s1340735 2:a86b09b00008 25 float filtered_emgT;
s1340735 14:bebdb2ac5ec6 26 float drempelwaardeT;
s1340735 2:a86b09b00008 27 int yT1, yT2;
Tanja2211 15:e779bfbeb8ea 28 float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,MOVAVG_T; //moving average objects
s1340735 14:bebdb2ac5ec6 29 float MOVAVG_Positie1, MOVAVG_Positie2;
s1340735 2:a86b09b00008 30 int positie;
s1340735 2:a86b09b00008 31
s1340735 0:abe0bc5c43b7 32
s1340735 0:abe0bc5c43b7 33 //*** FILTERS ***
Tanja2211 9:a1890454e5a7 34 arm_biquad_casd_df1_inst_f32 lowpassT;
Tanja2211 9:a1890454e5a7 35 arm_biquad_casd_df1_inst_f32 lowpassB;
s1340735 0:abe0bc5c43b7 36 //constants for 50Hz lowpass
Tanja2211 16:e997a6fd802a 37 float lowpass_const[] = {0, 0.558697707784438, 0.257307012382698, -0.292447944767337, 0.108452664934473};//{a0 a1 a2 -b1 -b2} van online calculator
s1340735 0:abe0bc5c43b7 38 float lowpass_states[4];
s1340735 0:abe0bc5c43b7 39
Tanja2211 9:a1890454e5a7 40 arm_biquad_casd_df1_inst_f32 highpassT;
Tanja2211 9:a1890454e5a7 41 arm_biquad_casd_df1_inst_f32 highpassB;
s1340735 0:abe0bc5c43b7 42 //constants for 10Hz highpass
Tanja2211 16:e997a6fd802a 43 float highpass_const[] = {1.000000000000000, -1.957555609146701, 0.957555609146701, -1.562244197866329, 0.641280516968023};//{a0 a1 a2 -b1 -b2}
s1340735 0:abe0bc5c43b7 44 float highpass_states[4];
s1340735 0:abe0bc5c43b7 45
Tanja2211 9:a1890454e5a7 46
Tanja2211 9:a1890454e5a7 47 // *** TRICEPS en BICEPS EMG ***
Tanja2211 9:a1890454e5a7 48 void Triceps()
Tanja2211 9:a1890454e5a7 49 {
Tanja2211 9:a1890454e5a7 50 //Triceps lezen
Tanja2211 9:a1890454e5a7 51 emg_valueT = emgT.read_u16();
Tanja2211 9:a1890454e5a7 52 emg_value_f32T = emgT.read();
Tanja2211 9:a1890454e5a7 53
Tanja2211 9:a1890454e5a7 54 //Triceps filteren
Tanja2211 9:a1890454e5a7 55 arm_biquad_cascade_df1_f32(&highpassT, &emg_value_f32T, &filtered_emgT, 1 );
Tanja2211 9:a1890454e5a7 56 arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
Tanja2211 9:a1890454e5a7 57 filtered_emgT = fabs(filtered_emgT);
Tanja2211 9:a1890454e5a7 58
Tanja2211 16:e997a6fd802a 59
Tanja2211 9:a1890454e5a7 60
Tanja2211 9:a1890454e5a7 61 //Triceps moving average
Tanja2211 9:a1890454e5a7 62 T0=filtered_emgT;
Tanja2211 15:e779bfbeb8ea 63 MOVAVG_T=T0*0.03333+T1*0.03333+T2*0.03333+T3*0.03333+T4*0.03333+T5*0.03333+T6*0.03333+T7*0.03333+T8*0.03333+T9*0.03333+T10*0.03333+T11*0.03333+T12*0.03333+T13*0.03333+T14*0.03333+T15*0.03333+T16*0.03333+T17*0.03333+T18*0.03333+T19*0.03333+T20*0.03333+T21*0.03333+T22*0.03333+T23*0.03333+T24*0.03333+T25*0.03333+T26*0.03333+T27*0.03333+T28*0.03333+T29*0.03333;
Tanja2211 15:e779bfbeb8ea 64 T29=T28;
Tanja2211 15:e779bfbeb8ea 65 T28=T27;
Tanja2211 15:e779bfbeb8ea 66 T27=T26;
Tanja2211 15:e779bfbeb8ea 67 T26=T25;
Tanja2211 15:e779bfbeb8ea 68 T25=T24;
Tanja2211 15:e779bfbeb8ea 69 T24=T23;
Tanja2211 15:e779bfbeb8ea 70 T23=T22;
Tanja2211 15:e779bfbeb8ea 71 T22=T21;
Tanja2211 15:e779bfbeb8ea 72 T21=T20;
Tanja2211 15:e779bfbeb8ea 73 T19=T18;
Tanja2211 15:e779bfbeb8ea 74 T18=T17;
Tanja2211 15:e779bfbeb8ea 75 T17=T16;
Tanja2211 15:e779bfbeb8ea 76 T16=T15;
Tanja2211 15:e779bfbeb8ea 77 T15=T14;
Tanja2211 15:e779bfbeb8ea 78 T14=T13;
Tanja2211 15:e779bfbeb8ea 79 T13=T12;
Tanja2211 15:e779bfbeb8ea 80 T12=T11;
Tanja2211 15:e779bfbeb8ea 81 T11=T10;
Tanja2211 15:e779bfbeb8ea 82 T10=T9;
Tanja2211 9:a1890454e5a7 83 T9=T8;
Tanja2211 9:a1890454e5a7 84 T8=T7;
Tanja2211 9:a1890454e5a7 85 T7=T6;
Tanja2211 9:a1890454e5a7 86 T6=T5;
Tanja2211 9:a1890454e5a7 87 T5=T4;
Tanja2211 9:a1890454e5a7 88 T4=T3;
Tanja2211 9:a1890454e5a7 89 T3=T2;
Tanja2211 9:a1890454e5a7 90 T2=T1;
Tanja2211 9:a1890454e5a7 91 T1=T0;
Tanja2211 16:e997a6fd802a 92
Tanja2211 16:e997a6fd802a 93 //sturen naar scherm (Realterm)
Tanja2211 16:e997a6fd802a 94 pc.printf("%f\r\n",MOVAVG_T);
Tanja2211 16:e997a6fd802a 95
s1340735 10:9319e872c752 96
s1340735 10:9319e872c752 97 //sturen naar HID Scope
Tanja2211 9:a1890454e5a7 98 scope.set(0,emg_valueT); //ruwe data
Tanja2211 9:a1890454e5a7 99 scope.set(1,filtered_emgT); //filtered
Tanja2211 9:a1890454e5a7 100 scope.send();
Tanja2211 9:a1890454e5a7 101 }
Tanja2211 9:a1890454e5a7 102
s1340735 0:abe0bc5c43b7 103 void Biceps()
s1340735 0:abe0bc5c43b7 104 {
Tanja2211 9:a1890454e5a7 105 //Biceps lezen
s1340735 0:abe0bc5c43b7 106 emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
s1340735 0:abe0bc5c43b7 107 emg_value_f32B = emgB.read();
s1340735 0:abe0bc5c43b7 108
Tanja2211 9:a1890454e5a7 109 //Biceps filteren
Tanja2211 9:a1890454e5a7 110 arm_biquad_cascade_df1_f32(&highpassB, &emg_value_f32B, &filtered_emgB, 1 );
Tanja2211 9:a1890454e5a7 111 arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
s1340735 0:abe0bc5c43b7 112 filtered_emgB = fabs(filtered_emgB);
s1340735 0:abe0bc5c43b7 113
Tanja2211 9:a1890454e5a7 114 //Biceps moving average
s1340735 0:abe0bc5c43b7 115 B0=filtered_emgB;
Tanja2211 15:e779bfbeb8ea 116 MOVAVG_B=B0*0.03333+B1*0.03333+B2*0.03333+B3*0.03333+B4*0.03333+B5*0.03333+B6*0.03333+B7*0.03333+B8*0.03333+B9*0.03333+B10*0.03333+B11*0.03333+B12*0.03333+B13*0.03333+B14*0.03333+B15*0.03333+B16*0.03333+B17*0.03333+B18*0.03333+B19*0.03333+B20*0.03333+B21*0.03333+B22*0.03333+B23*0.03333+B24*0.03333+B25*0.03333+B26*0.03333+B27*0.03333+B28*0.03333+B29*0.03333;
Tanja2211 15:e779bfbeb8ea 117 B29=B28;
Tanja2211 15:e779bfbeb8ea 118 B28=B27;
Tanja2211 15:e779bfbeb8ea 119 B27=B26;
Tanja2211 15:e779bfbeb8ea 120 B26=B25;
Tanja2211 15:e779bfbeb8ea 121 B25=B24;
Tanja2211 15:e779bfbeb8ea 122 B24=B23;
Tanja2211 15:e779bfbeb8ea 123 B23=B22;
Tanja2211 15:e779bfbeb8ea 124 B22=B21;
Tanja2211 15:e779bfbeb8ea 125 B21=B20;
Tanja2211 15:e779bfbeb8ea 126 B19=B18;
Tanja2211 15:e779bfbeb8ea 127 B18=B17;
Tanja2211 15:e779bfbeb8ea 128 B17=B16;
Tanja2211 15:e779bfbeb8ea 129 B16=B15;
Tanja2211 15:e779bfbeb8ea 130 B15=B14;
Tanja2211 15:e779bfbeb8ea 131 B14=B13;
Tanja2211 15:e779bfbeb8ea 132 B13=B12;
Tanja2211 15:e779bfbeb8ea 133 B12=B11;
Tanja2211 15:e779bfbeb8ea 134 B11=B10;
Tanja2211 15:e779bfbeb8ea 135 B10=B9;
s1340735 0:abe0bc5c43b7 136 B9=B8;
s1340735 0:abe0bc5c43b7 137 B8=B7;
s1340735 0:abe0bc5c43b7 138 B7=B6;
s1340735 0:abe0bc5c43b7 139 B6=B5;
s1340735 0:abe0bc5c43b7 140 B5=B4;
s1340735 0:abe0bc5c43b7 141 B4=B3;
s1340735 0:abe0bc5c43b7 142 B3=B2;
s1340735 0:abe0bc5c43b7 143 B2=B1;
s1340735 0:abe0bc5c43b7 144 B1=B0;
s1340735 0:abe0bc5c43b7 145
Tanja2211 16:e997a6fd802a 146 //sturen naar scherm
Tanja2211 16:e997a6fd802a 147 pc.printf("%f\r\n",MOVAVG_B);
Tanja2211 16:e997a6fd802a 148
s1340735 10:9319e872c752 149 //naar HID Scope
Tanja2211 6:ad6d4bd69205 150 scope.set(2,emg_valueB); //ruwe data
Tanja2211 6:ad6d4bd69205 151 scope.set(3,filtered_emgB); //filtered
Tanja2211 5:3232d78cb7b3 152 scope.send();
s1340735 2:a86b09b00008 153 }
s1340735 0:abe0bc5c43b7 154
s1340735 0:abe0bc5c43b7 155 // *** MAIN ***
s1340735 2:a86b09b00008 156 int main()
Tanja2211 9:a1890454e5a7 157 {
Tanja2211 9:a1890454e5a7 158 pc.baud(115200);
s1340735 10:9319e872c752 159
s1340735 14:bebdb2ac5ec6 160 //bepaling van positie met triceps 1
s1340735 14:bebdb2ac5ec6 161 Ticker log_timerT1;
Tanja2211 9:a1890454e5a7 162 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 9:a1890454e5a7 163 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 2:a86b09b00008 164
s1340735 14:bebdb2ac5ec6 165 log_timerT1.attach(Triceps, 0.005);
Tanja2211 16:e997a6fd802a 166 wait(30); //log_timerT wordt 2000 keer uitgevoerd
s1340735 14:bebdb2ac5ec6 167 log_timerT1.detach();
Tanja2211 15:e779bfbeb8ea 168
s1340735 14:bebdb2ac5ec6 169 MOVAVG_T=MOVAVG_Positie1;
Tanja2211 15:e779bfbeb8ea 170
s1340735 14:bebdb2ac5ec6 171 //bepaling van positie met tricep 2
s1340735 14:bebdb2ac5ec6 172 Ticker log_timerT2;
s1340735 14:bebdb2ac5ec6 173
s1340735 14:bebdb2ac5ec6 174 log_timerT2.attach(Triceps, 0.005);
Tanja2211 16:e997a6fd802a 175 wait(30);
s1340735 14:bebdb2ac5ec6 176 log_timerT2.detach();
Tanja2211 15:e779bfbeb8ea 177
s1340735 14:bebdb2ac5ec6 178 MOVAVG_T=MOVAVG_Positie2;
s1340735 14:bebdb2ac5ec6 179
s1340735 10:9319e872c752 180 // positie van batje met behulp van Triceps
s1340735 14:bebdb2ac5ec6 181 drempelwaardeT=0.0001;
s1340735 10:9319e872c752 182
s1340735 14:bebdb2ac5ec6 183 if (MOVAVG_Positie1 >= drempelwaardeT) {
s1340735 2:a86b09b00008 184 yT1=1;
s1340735 14:bebdb2ac5ec6 185 if (MOVAVG_Positie2 >= drempelwaardeT) {
s1340735 2:a86b09b00008 186 yT2=1;
s1340735 2:a86b09b00008 187 } else {
s1340735 2:a86b09b00008 188 yT2=0;
s1340735 2:a86b09b00008 189 }
s1340735 2:a86b09b00008 190 } else {
s1340735 2:a86b09b00008 191 yT1=0;
s1340735 2:a86b09b00008 192 }
Tanja2211 15:e779bfbeb8ea 193
s1340735 0:abe0bc5c43b7 194
s1340735 10:9319e872c752 195 //*** INPUT MOTOR 2 ***
s1340735 10:9319e872c752 196 positie=yT1+yT2;
s1340735 0:abe0bc5c43b7 197
s1340735 2:a86b09b00008 198 //controle positie op scherm
s1340735 2:a86b09b00008 199 if (positie==0) {
s1340735 2:a86b09b00008 200 pc.printf("Motor 2 gaat naar stand 0\n");
s1340735 2:a86b09b00008 201 } else {
s1340735 2:a86b09b00008 202 if (positie==1) {
s1340735 2:a86b09b00008 203 pc.printf("Motor 2 gaat naar stand 1\n");
s1340735 2:a86b09b00008 204 } else {
s1340735 2:a86b09b00008 205 if (positie==2) {
s1340735 2:a86b09b00008 206 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 2:a86b09b00008 207 }
s1340735 2:a86b09b00008 208 }
s1340735 2:a86b09b00008 209 }
s1340735 10:9319e872c752 210
s1340735 11:5044290660b0 211 wait(5);
s1340735 11:5044290660b0 212
s1340735 11:5044290660b0 213 Ticker log_timerB;
s1340735 11:5044290660b0 214 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
s1340735 11:5044290660b0 215 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
s1340735 11:5044290660b0 216
s1340735 11:5044290660b0 217 log_timerB.attach(Biceps,0.005);
Tanja2211 16:e997a6fd802a 218 wait(30);//log_timerB wordt 2000 keer uitgevoerd
s1340735 11:5044290660b0 219 log_timerB.detach();
s1340735 14:bebdb2ac5ec6 220
s1340735 2:a86b09b00008 221 //bepaling van snelheidsstand met biceps
Tanja2211 9:a1890454e5a7 222
s1340735 14:bebdb2ac5ec6 223 drempelwaardeB1=0.0001;
s1340735 14:bebdb2ac5ec6 224 drempelwaardeB2=0.001;
s1340735 14:bebdb2ac5ec6 225 drempelwaardeB3=0.01;
s1340735 2:a86b09b00008 226
s1340735 2:a86b09b00008 227 if (MOVAVG_B >= drempelwaardeB1) {
s1340735 2:a86b09b00008 228 yB1=1;
s1340735 2:a86b09b00008 229 if (MOVAVG_B >= drempelwaardeB2) {
s1340735 2:a86b09b00008 230 yB2=1;
s1340735 2:a86b09b00008 231 if (MOVAVG_B >= drempelwaardeB3) {
s1340735 2:a86b09b00008 232 yB3=1;
s1340735 0:abe0bc5c43b7 233 } else {
s1340735 2:a86b09b00008 234 yB3=0;
s1340735 0:abe0bc5c43b7 235 }
s1340735 0:abe0bc5c43b7 236 } else {
s1340735 2:a86b09b00008 237 yB2=0;
s1340735 0:abe0bc5c43b7 238 }
s1340735 2:a86b09b00008 239 } else {
s1340735 2:a86b09b00008 240 yB1=0;
s1340735 2:a86b09b00008 241 }
s1340735 0:abe0bc5c43b7 242
s1340735 10:9319e872c752 243 //*** INPUT MOTOR 1 ***
s1340735 10:9319e872c752 244 snelheidsstand=yB1+yB2+yB3;
s1340735 0:abe0bc5c43b7 245
s1340735 2:a86b09b00008 246 //controle snelheidsstand op scherm
s1340735 2:a86b09b00008 247 if (snelheidsstand==0) {
s1340735 2:a86b09b00008 248 pc.printf("Motor 1 beweegt niet\n");
s1340735 2:a86b09b00008 249 } else {
s1340735 1:6a8b45298e54 250 if (snelheidsstand==1) {
s1340735 1:6a8b45298e54 251 pc.printf("Motor 1 beweegt met snelheid 1\n");
s1340735 1:6a8b45298e54 252 } else {
s1340735 2:a86b09b00008 253 if (snelheidsstand==2) {
s1340735 2:a86b09b00008 254 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 2:a86b09b00008 255 } else {
s1340735 2:a86b09b00008 256 if (snelheidsstand==3) {
s1340735 2:a86b09b00008 257 pc.printf("Motor 1 beweegt met snelheid 3\n");
s1340735 2:a86b09b00008 258 }
s1340735 2:a86b09b00008 259 }
s1340735 1:6a8b45298e54 260 }
s1340735 1:6a8b45298e54 261 }
Tanja2211 9:a1890454e5a7 262
s1340735 10:9319e872c752 263 }