BMT M9 Groep01 / Mbed 2 deprecated test_emg

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:   HIDScope MODSERIAL mbed-dsp mbed TouchButton

Fork of test by BMT M9 Groep01

main.cpp@17:5fd768d0504f, 2014-10-24 (annotated)

Committer:
Tanja2211
Date:
Fri Oct 24 10:21:22 2014 +0000
Revision:
17:5fd768d0504f
Parent:
16:e997a6fd802a
Child:
18:1110be4aab6e
filters veranderd

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