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

Committer:
s1340735
Date:
Fri Oct 24 11:53:19 2014 +0000
Revision:
19:fdebe6892633
Parent:
18:1110be4aab6e
Child:
20:5cd52a0daab0
notch filter toegevoegd

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