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@38:017fd93e9efe, 2014-10-28 (annotated)

Committer:
Tanja2211
Date:
Tue Oct 28 13:45:16 2014 +0000
Revision:
38:017fd93e9efe
Parent:
37:8b15c29445d3
Child:
39:012ff1795e6b
Ik hoop dat het gaat werken

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 32:36bb6af01689 5 #include "mbed.h"
s1340735 32:36bb6af01689 6 #include "TouchButton.h"
s1340735 0:abe0bc5c43b7 7
s1340735 2:a86b09b00008 8 MODSERIAL pc(USBTX,USBRX);
s1340735 2:a86b09b00008 9
Tanja2211 9:a1890454e5a7 10 HIDScope scope(4);
s1340735 0:abe0bc5c43b7 11
s1340735 21:f0357a41f43e 12 AnalogIn emgB(PTB1); //biceps
s1340735 21:f0357a41f43e 13 AnalogIn emgT(PTB2); //tricep
s1340735 21:f0357a41f43e 14
s1340735 32:36bb6af01689 15 DigitalOut myled1(LED1);//red
s1340735 32:36bb6af01689 16 DigitalOut myled2(LED2);//green
s1340735 32:36bb6af01689 17 DigitalOut myled3(LED3);//blue
s1340735 32:36bb6af01689 18
s1340735 32:36bb6af01689 19 /* FRDM-KL25Z built-in touch slider
s1340735 36:ccc901c169b7 20 *******************
s1340735 36:ccc901c169b7 21 * * * *
s1340735 36:ccc901c169b7 22 * 1 * 2 * 3 *
s1340735 36:ccc901c169b7 23 * * * *
s1340735 36:ccc901c169b7 24 *******************
s1340735 33:bc0abe29bad1 25 * key 1 will light Red LED -> CALIBRATIE TRICEPS
s1340735 33:bc0abe29bad1 26 * key 2 will light Green LED -> CALIBRATIE BICEPS
s1340735 33:bc0abe29bad1 27 * key 3 will light Blue LED -> START
s1340735 32:36bb6af01689 28 */
s1340735 0:abe0bc5c43b7 29
s1340735 0:abe0bc5c43b7 30 //*** OBJECTS ***
s1340735 2:a86b09b00008 31 //bicep
s1340735 33:bc0abe29bad1 32 uint16_t emg_valueB;
s1340735 33:bc0abe29bad1 33 float emg_value_f32B;
s1340735 33:bc0abe29bad1 34 float filtered_emgB;
s1340735 0:abe0bc5c43b7 35 float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
s1340735 0:abe0bc5c43b7 36 int yB1, yB2, yB3;
s1340735 21:f0357a41f43e 37 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 21:f0357a41f43e 38 float B30, B31, B32, B33, B34, B35, B36, B37, B38, B39, B40, B41, B42, B43, B44, B45, B46, B47, B48, B49, B50, B51, B52, B53, B54, B55, B56, B57, B58, B59;
s1340735 1:6a8b45298e54 39 int snelheidsstand;
s1340735 2:a86b09b00008 40 //tricep
s1340735 33:bc0abe29bad1 41 uint16_t emg_valueT;
s1340735 33:bc0abe29bad1 42 float emg_value_f32T;
s1340735 33:bc0abe29bad1 43 float filtered_emgT;
s1340735 14:bebdb2ac5ec6 44 float drempelwaardeT;
s1340735 2:a86b09b00008 45 int yT1, yT2;
Tanja2211 15:e779bfbeb8ea 46 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 47 float MOVAVG_Positie1, MOVAVG_Positie2;
s1340735 2:a86b09b00008 48 int positie;
s1340735 2:a86b09b00008 49
s1340735 0:abe0bc5c43b7 50
s1340735 0:abe0bc5c43b7 51 //*** FILTERS ***
s1340735 19:fdebe6892633 52 arm_biquad_casd_df1_inst_f32 notchT;
s1340735 19:fdebe6892633 53 arm_biquad_casd_df1_inst_f32 notchB;
s1340735 21:f0357a41f43e 54 //constants for 50Hz
s1340735 21:f0357a41f43e 55 float notch_const[]= {0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122}; //{a0 a1 a2 -b1 -b2}
s1340735 19:fdebe6892633 56 float notch_states[4];
s1340735 19:fdebe6892633 57
Tanja2211 9:a1890454e5a7 58 arm_biquad_casd_df1_inst_f32 lowpassT;
Tanja2211 9:a1890454e5a7 59 arm_biquad_casd_df1_inst_f32 lowpassB;
s1340735 19:fdebe6892633 60 //constants for 60Hz lowpass
s1340735 19:fdebe6892633 61 float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator
s1340735 0:abe0bc5c43b7 62 float lowpass_states[4];
s1340735 0:abe0bc5c43b7 63
Tanja2211 9:a1890454e5a7 64 arm_biquad_casd_df1_inst_f32 highpassT;
Tanja2211 9:a1890454e5a7 65 arm_biquad_casd_df1_inst_f32 highpassB;
s1340735 19:fdebe6892633 66 //constants for 20Hz highpass
Tanja2211 17:5fd768d0504f 67 float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2}
s1340735 0:abe0bc5c43b7 68 float highpass_states[4];
s1340735 0:abe0bc5c43b7 69
Tanja2211 9:a1890454e5a7 70
Tanja2211 9:a1890454e5a7 71 // *** TRICEPS en BICEPS EMG ***
s1340735 34:514440ddca9a 72
Tanja2211 9:a1890454e5a7 73 void Triceps()
Tanja2211 9:a1890454e5a7 74 {
Tanja2211 9:a1890454e5a7 75 //Triceps lezen
Tanja2211 9:a1890454e5a7 76 emg_valueT = emgT.read_u16();
Tanja2211 9:a1890454e5a7 77 emg_value_f32T = emgT.read();
Tanja2211 9:a1890454e5a7 78
Tanja2211 9:a1890454e5a7 79 //Triceps filteren
Tanja2211 20:5cd52a0daab0 80 arm_biquad_cascade_df1_f32(&notchT, &emg_value_f32T, &filtered_emgT, 1);
s1340735 19:fdebe6892633 81 arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
Tanja2211 9:a1890454e5a7 82 filtered_emgT = fabs(filtered_emgT);
Tanja2211 17:5fd768d0504f 83 arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 );
Tanja2211 20:5cd52a0daab0 84 filtered_emgT = fabs(filtered_emgT);
s1340735 21:f0357a41f43e 85
Tanja2211 9:a1890454e5a7 86 //Triceps moving average
Tanja2211 28:c6b19fdfba01 87 T0=filtered_emgT*100;
Tanja2211 15:e779bfbeb8ea 88 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;
s1340735 33:bc0abe29bad1 89 T29=T28, T28=T27, T27=T26, T26=T25, T25=T24, T24=T23, T23=T22, T22=T21, T21=T20, T20=T19, T19=T18, T18=T17;
s1340735 33:bc0abe29bad1 90 T17=T16, T16=T15, T15=T14, T14=T13, T13=T12, T12=T11, T11=T10, T10=T9, T9=T8, T8=T7, T7=T6, T6=T5;
s1340735 33:bc0abe29bad1 91 T5=T4, T4=T3, T3=T2, T2=T1, T1=T0;
s1340735 21:f0357a41f43e 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 //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
s1340735 19:fdebe6892633 109 arm_biquad_cascade_df1_f32(&notchB, &emg_value_f32B, &filtered_emgB, 1 );
s1340735 19:fdebe6892633 110 arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
s1340735 0:abe0bc5c43b7 111 filtered_emgB = fabs(filtered_emgB);
Tanja2211 17:5fd768d0504f 112 arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 );
Tanja2211 20:5cd52a0daab0 113 filtered_emgB = fabs(filtered_emgB);
s1340735 21:f0357a41f43e 114
Tanja2211 9:a1890454e5a7 115 //Biceps moving average
Tanja2211 22:0be5dfa04f75 116 B0=filtered_emgB*1000;
s1340735 21:f0357a41f43e 117 MOVAVG_B=B0*0.01667+B1*0.01667+B2*0.01667+B3*0.01667+B4*0.01667+B5*0.01667+B6*0.01667+B7*0.01667+B8*0.01667+B9*0.01667+B10*0.01667+B11*0.01667+B12*0.01667+B13*0.01667+B14*0.01667+B15*0.01667+B16*0.01667+B17*0.01667+B18*0.01667+B19*0.01667+B20*0.01667+B21*0.01667+B22*0.01667+B23*0.01667+B24*0.01667+B25*0.01667+B26*0.01667+B27*0.01667+B28*0.01667+B29*0.01667+B30*0.01667+B31*0.01667+B32*0.01667+B33*0.01667+B34*0.01667+B35*0.01667+B36*0.01667+B37*0.01667+B38*0.01667+B39*0.01667+B40*0.01667+B41*0.01667+B42*0.01667+B43*0.01667+B44*0.01667+B45*0.01667+B46*0.01667+B47*0.01667+B48*0.01667+B49*0.01667+B50*0.01667+B51*0.01667+B52*0.01667+B53*0.01667+B54*0.01667+B55*0.01667+B56*0.01667+B57*0.01667+B58*0.01667+B59*0.01667;
s1340735 33:bc0abe29bad1 118 B59=B58, B58=B57, B57=B56, B56=B55, B55=B54, B54=B53, B53=B52, B52=B51, B51=B50, B50=B48, B49=B49;
s1340735 33:bc0abe29bad1 119 B48=B47, B47=B46, B46=B45, B45=B44, B43=B42, B42=B41, B41=B40, B40=B39, B39=B38, B38=B37, B37=B36;
s1340735 33:bc0abe29bad1 120 B36=B35, B35=B34, B34=B33, B33=B32, B32=B31, B31=B30, B30=B29, B29=B28, B28=B27, B27=B26, B26=B25;
s1340735 33:bc0abe29bad1 121 B25=B24, B24=B23, B23=B22, B22=B21, B21=B20, B20=B19, B19=B18, B18=B17, B17=B16, B16=B15, B15=B14;
s1340735 33:bc0abe29bad1 122 B14=B13, B13=B12, B12=B11, B11=B10, B10=B9, B9=B8, B8=B7, B7=B6, B6=B5, B5=B4, B4=B3;
s1340735 33:bc0abe29bad1 123 B3=B2, B2=B1, B1=B0;
s1340735 0:abe0bc5c43b7 124
s1340735 32:36bb6af01689 125 //sturen naar scherm
Tanja2211 16:e997a6fd802a 126 pc.printf("%f\r\n",MOVAVG_B);
Tanja2211 16:e997a6fd802a 127
s1340735 10:9319e872c752 128 //naar HID Scope
Tanja2211 6:ad6d4bd69205 129 scope.set(2,emg_valueB); //ruwe data
Tanja2211 6:ad6d4bd69205 130 scope.set(3,filtered_emgB); //filtered
Tanja2211 5:3232d78cb7b3 131 scope.send();
s1340735 2:a86b09b00008 132 }
s1340735 0:abe0bc5c43b7 133
s1340735 19:fdebe6892633 134
s1340735 32:36bb6af01689 135 // *** TRICEPS EN BICEPS CALIBRATIE ***
s1340735 32:36bb6af01689 136 void Calibratie_Triceps()
Tanja2211 9:a1890454e5a7 137 {
s1340735 32:36bb6af01689 138 //triceps drempelwaarde calibreren
s1340735 10:9319e872c752 139
s1340735 32:36bb6af01689 140 Ticker log_timerT;
s1340735 21:f0357a41f43e 141
s1340735 19:fdebe6892633 142 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 9:a1890454e5a7 143 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 9:a1890454e5a7 144 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 2:a86b09b00008 145
s1340735 32:36bb6af01689 146 log_timerT.attach(Triceps, 0.005);
s1340735 32:36bb6af01689 147 wait(3);
s1340735 32:36bb6af01689 148 log_timerT.detach();
s1340735 32:36bb6af01689 149 }
s1340735 10:9319e872c752 150
s1340735 32:36bb6af01689 151 void Calibratie_Biceps()
s1340735 32:36bb6af01689 152 {
s1340735 32:36bb6af01689 153 Ticker log_timerB;
Tanja2211 26:e43cae7186f5 154
Tanja2211 26:e43cae7186f5 155 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 26:e43cae7186f5 156 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 26:e43cae7186f5 157 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
Tanja2211 26:e43cae7186f5 158
s1340735 32:36bb6af01689 159 log_timerB.attach(Biceps, 0.005);
s1340735 34:514440ddca9a 160 wait(5);
s1340735 32:36bb6af01689 161 log_timerB.detach();
s1340735 32:36bb6af01689 162 }
s1340735 32:36bb6af01689 163
s1340735 32:36bb6af01689 164 // *** MAIN ***
s1340735 32:36bb6af01689 165 int main()
s1340735 32:36bb6af01689 166 {
s1340735 32:36bb6af01689 167 pc.baud(115200);
s1340735 32:36bb6af01689 168
s1340735 34:514440ddca9a 169 drempelwaardeT=0;
s1340735 34:514440ddca9a 170 drempelwaardeB1=0;
s1340735 34:514440ddca9a 171 drempelwaardeB2=0;
s1340735 34:514440ddca9a 172 drempelwaardeB3=0;
s1340735 35:e60f09b575d7 173
s1340735 36:ccc901c169b7 174 TouchButton TButton;
s1340735 36:ccc901c169b7 175
s1340735 37:8b15c29445d3 176 int key=0;//vraagt om calibratie
s1340735 36:ccc901c169b7 177 key = TButton.PressedButton();
s1340735 32:36bb6af01689 178
s1340735 32:36bb6af01689 179 pc.printf("key 1 calibratie triceps/n");
s1340735 32:36bb6af01689 180 pc.printf("key 2 caliratie biceps/n");
s1340735 32:36bb6af01689 181 pc.printf("key 3 START/n");
Tanja2211 26:e43cae7186f5 182
s1340735 37:8b15c29445d3 183 wait(5);
s1340735 37:8b15c29445d3 184
Tanja2211 38:017fd93e9efe 185 if (key==1) {
Tanja2211 38:017fd93e9efe 186
Tanja2211 38:017fd93e9efe 187 //rood
Tanja2211 38:017fd93e9efe 188 myled1 = 0;
Tanja2211 38:017fd93e9efe 189 myled2 = 1;
Tanja2211 38:017fd93e9efe 190 myled3 = 1;
Tanja2211 38:017fd93e9efe 191
Tanja2211 38:017fd93e9efe 192 pc.printf("calibratie tricep aan\n");
Tanja2211 38:017fd93e9efe 193 wait(2);
Tanja2211 38:017fd93e9efe 194
Tanja2211 38:017fd93e9efe 195 Calibratie_Triceps();
Tanja2211 38:017fd93e9efe 196 drempelwaardeT=MOVAVG_T-1;
Tanja2211 38:017fd93e9efe 197 pc.printf("drempelwaarde triceps is %f\r\n", drempelwaardeT);
Tanja2211 38:017fd93e9efe 198 pc.printf("calibratie tricep klaar,\n");
Tanja2211 38:017fd93e9efe 199 wait(5);
Tanja2211 38:017fd93e9efe 200
Tanja2211 38:017fd93e9efe 201 //pc.printf("key 2 caliratie biceps/n");
Tanja2211 38:017fd93e9efe 202 //pc.printf("key 3 START/n");
Tanja2211 38:017fd93e9efe 203 //wait(5);
Tanja2211 38:017fd93e9efe 204
Tanja2211 38:017fd93e9efe 205 } else if (key==2) {
Tanja2211 38:017fd93e9efe 206
Tanja2211 38:017fd93e9efe 207 //green
Tanja2211 38:017fd93e9efe 208 myled1 = 1;
Tanja2211 38:017fd93e9efe 209 myled2 = 0;
Tanja2211 38:017fd93e9efe 210 myled3 = 1;
Tanja2211 38:017fd93e9efe 211 wait(0.1);
Tanja2211 38:017fd93e9efe 212
Tanja2211 38:017fd93e9efe 213 pc.printf("calibratie bicep snelheid 1 aan\n");
Tanja2211 38:017fd93e9efe 214 wait(2);
s1340735 36:ccc901c169b7 215
Tanja2211 38:017fd93e9efe 216 Calibratie_Biceps();
Tanja2211 38:017fd93e9efe 217 drempelwaardeB1=MOVAVG_B-1;
Tanja2211 38:017fd93e9efe 218 pc.printf("drempelwaarde snelheid 1 is %f\r\n", drempelwaardeB1);
Tanja2211 38:017fd93e9efe 219 wait(5);
Tanja2211 38:017fd93e9efe 220
Tanja2211 38:017fd93e9efe 221 pc.printf("calibratie bicep snelheid 2 aan\n");
Tanja2211 38:017fd93e9efe 222 wait(2);
Tanja2211 38:017fd93e9efe 223
Tanja2211 38:017fd93e9efe 224 Calibratie_Biceps();
Tanja2211 38:017fd93e9efe 225 drempelwaardeB2=MOVAVG_B-1;
Tanja2211 38:017fd93e9efe 226 pc.printf("drempelwaarde snelheid 2 is %f\r\n", drempelwaardeB2);
Tanja2211 38:017fd93e9efe 227 wait(5);
Tanja2211 38:017fd93e9efe 228
Tanja2211 38:017fd93e9efe 229 pc.printf("calibratie bicep snelheid 2 aan\n");
Tanja2211 38:017fd93e9efe 230 wait(2);
Tanja2211 38:017fd93e9efe 231
Tanja2211 38:017fd93e9efe 232 Calibratie_Biceps();
Tanja2211 38:017fd93e9efe 233 wait(2);
Tanja2211 38:017fd93e9efe 234
Tanja2211 38:017fd93e9efe 235 drempelwaardeB2=MOVAVG_B-1;
Tanja2211 38:017fd93e9efe 236 pc.printf("drempelwaarde snelheid 3 is %f\r\n", drempelwaardeB3);
Tanja2211 38:017fd93e9efe 237
Tanja2211 38:017fd93e9efe 238 pc.printf("caliratie biceps is klaar, accord? -> key 4/n");
Tanja2211 38:017fd93e9efe 239
Tanja2211 38:017fd93e9efe 240 wait(5);
Tanja2211 38:017fd93e9efe 241 } else if (key==3) {
Tanja2211 38:017fd93e9efe 242 if(drempelwaardeT==0) {
Tanja2211 38:017fd93e9efe 243 pc.printf("voer calibratie triceps uit");
Tanja2211 38:017fd93e9efe 244 } else if (drempelwaardeB1==0) {
Tanja2211 38:017fd93e9efe 245 pc.printf("voer calibratie biceps uit");
Tanja2211 38:017fd93e9efe 246 } else if (drempelwaardeB2==0) {
Tanja2211 38:017fd93e9efe 247 pc.printf("voer calibratie biceps uit");
Tanja2211 38:017fd93e9efe 248 } else if (drempelwaardeB3==0) {
Tanja2211 38:017fd93e9efe 249 pc.printf("voer calibratie biceps uit");
Tanja2211 38:017fd93e9efe 250 } else {
Tanja2211 38:017fd93e9efe 251
Tanja2211 38:017fd93e9efe 252 //blue
s1340735 37:8b15c29445d3 253 myled1 = 1;
s1340735 37:8b15c29445d3 254 myled2 = 1;
Tanja2211 38:017fd93e9efe 255 myled3 = 0;
Tanja2211 38:017fd93e9efe 256 wait(0.1);
Tanja2211 38:017fd93e9efe 257
Tanja2211 38:017fd93e9efe 258 pc.printf("eerst positie dan snelheid aangeven/n");
s1340735 37:8b15c29445d3 259
Tanja2211 38:017fd93e9efe 260 //bepaling van positie met triceps 1
Tanja2211 38:017fd93e9efe 261 Ticker log_timerT1;
Tanja2211 38:017fd93e9efe 262
Tanja2211 38:017fd93e9efe 263 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 38:017fd93e9efe 264 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 38:017fd93e9efe 265 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 37:8b15c29445d3 266
Tanja2211 38:017fd93e9efe 267 log_timerT1.attach(Triceps, 0.005);
Tanja2211 38:017fd93e9efe 268 wait(10);
Tanja2211 38:017fd93e9efe 269 log_timerT1.detach();
Tanja2211 38:017fd93e9efe 270
Tanja2211 38:017fd93e9efe 271 MOVAVG_T=MOVAVG_Positie1;
Tanja2211 38:017fd93e9efe 272
Tanja2211 38:017fd93e9efe 273 // positie van batje met behulp van Triceps
s1340735 36:ccc901c169b7 274
Tanja2211 38:017fd93e9efe 275 if (MOVAVG_Positie1>= drempelwaardeT) {
Tanja2211 38:017fd93e9efe 276 yT1=1;
Tanja2211 38:017fd93e9efe 277 } else {
Tanja2211 38:017fd93e9efe 278 yT1=0;
Tanja2211 38:017fd93e9efe 279 }
s1340735 37:8b15c29445d3 280
Tanja2211 38:017fd93e9efe 281 pc.printf("Triceps eerste meting is klaar.\n");
s1340735 37:8b15c29445d3 282 wait(5);
s1340735 37:8b15c29445d3 283
Tanja2211 38:017fd93e9efe 284 //bepaling van positie met tricep 2
Tanja2211 38:017fd93e9efe 285 Ticker log_timerT2;
s1340735 37:8b15c29445d3 286
Tanja2211 38:017fd93e9efe 287 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 38:017fd93e9efe 288 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 38:017fd93e9efe 289 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 32:36bb6af01689 290
Tanja2211 38:017fd93e9efe 291 log_timerT2.attach(Triceps, 0.005);
s1340735 37:8b15c29445d3 292 wait(5);
Tanja2211 38:017fd93e9efe 293 log_timerT2.detach();
s1340735 37:8b15c29445d3 294
Tanja2211 38:017fd93e9efe 295 MOVAVG_T=MOVAVG_Positie2;
s1340735 35:e60f09b575d7 296
Tanja2211 38:017fd93e9efe 297 if (MOVAVG_Positie2 >= drempelwaardeT) {
Tanja2211 38:017fd93e9efe 298 yT2=1;
Tanja2211 38:017fd93e9efe 299 } else {
Tanja2211 38:017fd93e9efe 300 yT2=0;
Tanja2211 38:017fd93e9efe 301 }
Tanja2211 38:017fd93e9efe 302
Tanja2211 38:017fd93e9efe 303 pc.printf("Triceps tweede meting is klaar.\n");
s1340735 37:8b15c29445d3 304
Tanja2211 38:017fd93e9efe 305 //*** INPUT MOTOR 2 ***
Tanja2211 38:017fd93e9efe 306 positie=yT1+yT2;
s1340735 37:8b15c29445d3 307
Tanja2211 38:017fd93e9efe 308 //controle positie op scherm
Tanja2211 38:017fd93e9efe 309 if (positie==0) {
Tanja2211 38:017fd93e9efe 310 pc.printf("Motor 2 blijft op stand 1\n");
Tanja2211 38:017fd93e9efe 311 } else {
Tanja2211 38:017fd93e9efe 312 if (positie==1) {
Tanja2211 38:017fd93e9efe 313 pc.printf("Motor 2 gaat naar stand 2\n");
Tanja2211 38:017fd93e9efe 314 } else {
Tanja2211 38:017fd93e9efe 315 if (positie==2) {
Tanja2211 38:017fd93e9efe 316 pc.printf("Motor 2 gaat naar stand 3\n");
Tanja2211 38:017fd93e9efe 317 }
Tanja2211 38:017fd93e9efe 318 }
Tanja2211 38:017fd93e9efe 319 }
s1340735 36:ccc901c169b7 320
s1340735 37:8b15c29445d3 321 wait(5);
s1340735 35:e60f09b575d7 322
Tanja2211 38:017fd93e9efe 323 Ticker log_timerB;
s1340735 37:8b15c29445d3 324
Tanja2211 38:017fd93e9efe 325 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
Tanja2211 38:017fd93e9efe 326 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
Tanja2211 38:017fd93e9efe 327 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
s1340735 37:8b15c29445d3 328
Tanja2211 38:017fd93e9efe 329 log_timerB.attach(Biceps,0.005);
Tanja2211 38:017fd93e9efe 330 wait(5);
Tanja2211 38:017fd93e9efe 331 log_timerB.detach();
s1340735 37:8b15c29445d3 332
Tanja2211 38:017fd93e9efe 333 //bepaling van snelheidsstand met biceps
s1340735 37:8b15c29445d3 334
Tanja2211 38:017fd93e9efe 335 if (MOVAVG_B >= drempelwaardeB1) {
Tanja2211 38:017fd93e9efe 336 yB1=1;
Tanja2211 38:017fd93e9efe 337 if (MOVAVG_B >= drempelwaardeB2) {
Tanja2211 38:017fd93e9efe 338 yB2=1;
Tanja2211 38:017fd93e9efe 339 if (MOVAVG_B >= drempelwaardeB3) {
Tanja2211 38:017fd93e9efe 340 yB3=1;
s1340735 37:8b15c29445d3 341 } else {
Tanja2211 38:017fd93e9efe 342 yB3=0;
s1340735 37:8b15c29445d3 343 }
s1340735 37:8b15c29445d3 344 } else {
Tanja2211 38:017fd93e9efe 345 yB2=0;
s1340735 37:8b15c29445d3 346 }
Tanja2211 38:017fd93e9efe 347 } else {
Tanja2211 38:017fd93e9efe 348 yB1=0;
Tanja2211 38:017fd93e9efe 349 }
s1340735 36:ccc901c169b7 350
Tanja2211 38:017fd93e9efe 351 //*** INPUT MOTOR 1 ***
Tanja2211 38:017fd93e9efe 352 snelheidsstand=yB1+yB2+yB3;
s1340735 32:36bb6af01689 353
Tanja2211 38:017fd93e9efe 354 //controle snelheidsstand op scherm
Tanja2211 38:017fd93e9efe 355 if (snelheidsstand==0) {
Tanja2211 38:017fd93e9efe 356 pc.printf("Motor 1 beweegt niet\n");
Tanja2211 38:017fd93e9efe 357 } else {
Tanja2211 38:017fd93e9efe 358 if (snelheidsstand==1) {
Tanja2211 38:017fd93e9efe 359 pc.printf("Motor 1 beweegt met snelheid 1\n");
s1340735 37:8b15c29445d3 360 } else {
Tanja2211 38:017fd93e9efe 361 if (snelheidsstand==2) {
Tanja2211 38:017fd93e9efe 362 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 37:8b15c29445d3 363 } else {
Tanja2211 38:017fd93e9efe 364 if (snelheidsstand==3) {
Tanja2211 38:017fd93e9efe 365 pc.printf("Motor 1 beweegt met snelheid 3\n");
s1340735 32:36bb6af01689 366 }
s1340735 32:36bb6af01689 367 }
s1340735 32:36bb6af01689 368 }
s1340735 32:36bb6af01689 369 }
s1340735 35:e60f09b575d7 370 }
s1340735 36:ccc901c169b7 371 }
Tanja2211 38:017fd93e9efe 372
s1340735 37:8b15c29445d3 373 }