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@15:e779bfbeb8ea, 2014-10-24 (annotated)

Committer:
Tanja2211
Date:
Fri Oct 24 08:08:03 2014 +0000
Revision:
15:e779bfbeb8ea
Parent:
14:bebdb2ac5ec6
Child:
16:e997a6fd802a
Moving average 30

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
s1340735 13:9071a6b3c886 37 float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, 0, 0.17156822136};//{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
s1340735 13:9071a6b3c886 43 float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, -1.5610153913, 0.6413487154};//{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
s1340735 10:9319e872c752 59 //sturen naar scherm (Realterm)
s1340735 14:bebdb2ac5ec6 60 pc.printf("%f\r\n",filtered_emgT);
Tanja2211 9:a1890454e5a7 61
Tanja2211 9:a1890454e5a7 62 //Triceps moving average
Tanja2211 9:a1890454e5a7 63 T0=filtered_emgT;
Tanja2211 15:e779bfbeb8ea 64 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 65 T29=T28;
Tanja2211 15:e779bfbeb8ea 66 T28=T27;
Tanja2211 15:e779bfbeb8ea 67 T27=T26;
Tanja2211 15:e779bfbeb8ea 68 T26=T25;
Tanja2211 15:e779bfbeb8ea 69 T25=T24;
Tanja2211 15:e779bfbeb8ea 70 T24=T23;
Tanja2211 15:e779bfbeb8ea 71 T23=T22;
Tanja2211 15:e779bfbeb8ea 72 T22=T21;
Tanja2211 15:e779bfbeb8ea 73 T21=T20;
Tanja2211 15:e779bfbeb8ea 74 T19=T18;
Tanja2211 15:e779bfbeb8ea 75 T18=T17;
Tanja2211 15:e779bfbeb8ea 76 T17=T16;
Tanja2211 15:e779bfbeb8ea 77 T16=T15;
Tanja2211 15:e779bfbeb8ea 78 T15=T14;
Tanja2211 15:e779bfbeb8ea 79 T14=T13;
Tanja2211 15:e779bfbeb8ea 80 T13=T12;
Tanja2211 15:e779bfbeb8ea 81 T12=T11;
Tanja2211 15:e779bfbeb8ea 82 T11=T10;
Tanja2211 15:e779bfbeb8ea 83 T10=T9;
Tanja2211 9:a1890454e5a7 84 T9=T8;
Tanja2211 9:a1890454e5a7 85 T8=T7;
Tanja2211 9:a1890454e5a7 86 T7=T6;
Tanja2211 9:a1890454e5a7 87 T6=T5;
Tanja2211 9:a1890454e5a7 88 T5=T4;
Tanja2211 9:a1890454e5a7 89 T4=T3;
Tanja2211 9:a1890454e5a7 90 T3=T2;
Tanja2211 9:a1890454e5a7 91 T2=T1;
Tanja2211 9:a1890454e5a7 92 T1=T0;
s1340735 10:9319e872c752 93
s1340735 10:9319e872c752 94 //sturen naar HID Scope
Tanja2211 9:a1890454e5a7 95 scope.set(0,emg_valueT); //ruwe data
Tanja2211 9:a1890454e5a7 96 scope.set(1,filtered_emgT); //filtered
Tanja2211 9:a1890454e5a7 97 scope.send();
Tanja2211 9:a1890454e5a7 98 }
Tanja2211 9:a1890454e5a7 99
s1340735 0:abe0bc5c43b7 100 void Biceps()
s1340735 0:abe0bc5c43b7 101 {
Tanja2211 9:a1890454e5a7 102 //Biceps lezen
s1340735 0:abe0bc5c43b7 103 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 104 emg_value_f32B = emgB.read();
s1340735 0:abe0bc5c43b7 105
Tanja2211 9:a1890454e5a7 106 //Biceps filteren
Tanja2211 9:a1890454e5a7 107 arm_biquad_cascade_df1_f32(&highpassB, &emg_value_f32B, &filtered_emgB, 1 );
Tanja2211 9:a1890454e5a7 108 arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
s1340735 0:abe0bc5c43b7 109 filtered_emgB = fabs(filtered_emgB);
s1340735 0:abe0bc5c43b7 110
s1340735 10:9319e872c752 111 //sturen naar scherm
Tanja2211 9:a1890454e5a7 112 pc.printf("%f\r\n",filtered_emgB);
Tanja2211 9:a1890454e5a7 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
s1340735 10:9319e872c752 146 //naar HID Scope
Tanja2211 6:ad6d4bd69205 147 scope.set(2,emg_valueB); //ruwe data
Tanja2211 6:ad6d4bd69205 148 scope.set(3,filtered_emgB); //filtered
Tanja2211 5:3232d78cb7b3 149 scope.send();
s1340735 2:a86b09b00008 150 }
s1340735 0:abe0bc5c43b7 151
s1340735 0:abe0bc5c43b7 152 // *** MAIN ***
s1340735 2:a86b09b00008 153 int main()
Tanja2211 9:a1890454e5a7 154 {
Tanja2211 9:a1890454e5a7 155 pc.baud(115200);
s1340735 10:9319e872c752 156
s1340735 14:bebdb2ac5ec6 157 //bepaling van positie met triceps 1
s1340735 14:bebdb2ac5ec6 158 Ticker log_timerT1;
Tanja2211 9:a1890454e5a7 159 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 9:a1890454e5a7 160 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 2:a86b09b00008 161
s1340735 14:bebdb2ac5ec6 162 log_timerT1.attach(Triceps, 0.005);
s1340735 11:5044290660b0 163 wait(60); //log_timerT wordt 2000 keer uitgevoerd
s1340735 14:bebdb2ac5ec6 164 log_timerT1.detach();
Tanja2211 15:e779bfbeb8ea 165
s1340735 14:bebdb2ac5ec6 166 MOVAVG_T=MOVAVG_Positie1;
Tanja2211 15:e779bfbeb8ea 167
s1340735 14:bebdb2ac5ec6 168 //bepaling van positie met tricep 2
s1340735 14:bebdb2ac5ec6 169 Ticker log_timerT2;
s1340735 14:bebdb2ac5ec6 170
s1340735 14:bebdb2ac5ec6 171 log_timerT2.attach(Triceps, 0.005);
s1340735 14:bebdb2ac5ec6 172 wait(60);
s1340735 14:bebdb2ac5ec6 173 log_timerT2.detach();
Tanja2211 15:e779bfbeb8ea 174
s1340735 14:bebdb2ac5ec6 175 MOVAVG_T=MOVAVG_Positie2;
s1340735 14:bebdb2ac5ec6 176
s1340735 10:9319e872c752 177 // positie van batje met behulp van Triceps
s1340735 14:bebdb2ac5ec6 178 drempelwaardeT=0.0001;
s1340735 10:9319e872c752 179
s1340735 14:bebdb2ac5ec6 180 if (MOVAVG_Positie1 >= drempelwaardeT) {
s1340735 2:a86b09b00008 181 yT1=1;
s1340735 14:bebdb2ac5ec6 182 if (MOVAVG_Positie2 >= drempelwaardeT) {
s1340735 2:a86b09b00008 183 yT2=1;
s1340735 2:a86b09b00008 184 } else {
s1340735 2:a86b09b00008 185 yT2=0;
s1340735 2:a86b09b00008 186 }
s1340735 2:a86b09b00008 187 } else {
s1340735 2:a86b09b00008 188 yT1=0;
s1340735 2:a86b09b00008 189 }
Tanja2211 15:e779bfbeb8ea 190
s1340735 0:abe0bc5c43b7 191
s1340735 10:9319e872c752 192 //*** INPUT MOTOR 2 ***
s1340735 10:9319e872c752 193 positie=yT1+yT2;
s1340735 0:abe0bc5c43b7 194
s1340735 2:a86b09b00008 195 //controle positie op scherm
s1340735 2:a86b09b00008 196 if (positie==0) {
s1340735 2:a86b09b00008 197 pc.printf("Motor 2 gaat naar stand 0\n");
s1340735 2:a86b09b00008 198 } else {
s1340735 2:a86b09b00008 199 if (positie==1) {
s1340735 2:a86b09b00008 200 pc.printf("Motor 2 gaat naar stand 1\n");
s1340735 2:a86b09b00008 201 } else {
s1340735 2:a86b09b00008 202 if (positie==2) {
s1340735 2:a86b09b00008 203 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 2:a86b09b00008 204 }
s1340735 2:a86b09b00008 205 }
s1340735 2:a86b09b00008 206 }
s1340735 10:9319e872c752 207
s1340735 11:5044290660b0 208 wait(5);
s1340735 11:5044290660b0 209
s1340735 11:5044290660b0 210 Ticker log_timerB;
s1340735 11:5044290660b0 211 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
s1340735 11:5044290660b0 212 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
s1340735 11:5044290660b0 213
s1340735 11:5044290660b0 214 log_timerB.attach(Biceps,0.005);
s1340735 11:5044290660b0 215 wait(60);//log_timerB wordt 2000 keer uitgevoerd
s1340735 11:5044290660b0 216 log_timerB.detach();
s1340735 14:bebdb2ac5ec6 217
s1340735 2:a86b09b00008 218 //bepaling van snelheidsstand met biceps
Tanja2211 9:a1890454e5a7 219
s1340735 14:bebdb2ac5ec6 220 drempelwaardeB1=0.0001;
s1340735 14:bebdb2ac5ec6 221 drempelwaardeB2=0.001;
s1340735 14:bebdb2ac5ec6 222 drempelwaardeB3=0.01;
s1340735 2:a86b09b00008 223
s1340735 2:a86b09b00008 224 if (MOVAVG_B >= drempelwaardeB1) {
s1340735 2:a86b09b00008 225 yB1=1;
s1340735 2:a86b09b00008 226 if (MOVAVG_B >= drempelwaardeB2) {
s1340735 2:a86b09b00008 227 yB2=1;
s1340735 2:a86b09b00008 228 if (MOVAVG_B >= drempelwaardeB3) {
s1340735 2:a86b09b00008 229 yB3=1;
s1340735 0:abe0bc5c43b7 230 } else {
s1340735 2:a86b09b00008 231 yB3=0;
s1340735 0:abe0bc5c43b7 232 }
s1340735 0:abe0bc5c43b7 233 } else {
s1340735 2:a86b09b00008 234 yB2=0;
s1340735 0:abe0bc5c43b7 235 }
s1340735 2:a86b09b00008 236 } else {
s1340735 2:a86b09b00008 237 yB1=0;
s1340735 2:a86b09b00008 238 }
s1340735 0:abe0bc5c43b7 239
s1340735 10:9319e872c752 240 //*** INPUT MOTOR 1 ***
s1340735 10:9319e872c752 241 snelheidsstand=yB1+yB2+yB3;
s1340735 0:abe0bc5c43b7 242
s1340735 2:a86b09b00008 243 //controle snelheidsstand op scherm
s1340735 2:a86b09b00008 244 if (snelheidsstand==0) {
s1340735 2:a86b09b00008 245 pc.printf("Motor 1 beweegt niet\n");
s1340735 2:a86b09b00008 246 } else {
s1340735 1:6a8b45298e54 247 if (snelheidsstand==1) {
s1340735 1:6a8b45298e54 248 pc.printf("Motor 1 beweegt met snelheid 1\n");
s1340735 1:6a8b45298e54 249 } else {
s1340735 2:a86b09b00008 250 if (snelheidsstand==2) {
s1340735 2:a86b09b00008 251 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 2:a86b09b00008 252 } else {
s1340735 2:a86b09b00008 253 if (snelheidsstand==3) {
s1340735 2:a86b09b00008 254 pc.printf("Motor 1 beweegt met snelheid 3\n");
s1340735 2:a86b09b00008 255 }
s1340735 2:a86b09b00008 256 }
s1340735 1:6a8b45298e54 257 }
s1340735 1:6a8b45298e54 258 }
Tanja2211 9:a1890454e5a7 259
s1340735 10:9319e872c752 260 }