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@42:6430dd3a8ea2, 2014-10-28 (annotated)

Committer:
Tanja2211
Date:
Tue Oct 28 15:38:58 2014 +0000
Revision:
42:6430dd3a8ea2
Parent:
41:d86fced533a8
Child:
43:bc93d31b80f5
het werkt

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();
Tanja2211 41:d86fced533a8 107 pc.printf("a1\n");
Tanja2211 9:a1890454e5a7 108 //Biceps filteren
s1340735 19:fdebe6892633 109 arm_biquad_cascade_df1_f32(&notchB, &emg_value_f32B, &filtered_emgB, 1 );
Tanja2211 41:d86fced533a8 110 pc.printf("a12\n");
s1340735 19:fdebe6892633 111 arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
Tanja2211 41:d86fced533a8 112 pc.printf("a13\n");
s1340735 0:abe0bc5c43b7 113 filtered_emgB = fabs(filtered_emgB);
Tanja2211 41:d86fced533a8 114 pc.printf("a14\n");
Tanja2211 17:5fd768d0504f 115 arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 );
Tanja2211 41:d86fced533a8 116 pc.printf("a15\n");
Tanja2211 20:5cd52a0daab0 117 filtered_emgB = fabs(filtered_emgB);
Tanja2211 41:d86fced533a8 118 pc.printf("a2\n");
Tanja2211 9:a1890454e5a7 119 //Biceps moving average
Tanja2211 22:0be5dfa04f75 120 B0=filtered_emgB*1000;
s1340735 21:f0357a41f43e 121 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 122 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 123 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 124 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 125 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 126 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 127 B3=B2, B2=B1, B1=B0;
Tanja2211 41:d86fced533a8 128 pc.printf("a3\n");
s1340735 32:36bb6af01689 129 //sturen naar scherm
Tanja2211 16:e997a6fd802a 130 pc.printf("%f\r\n",MOVAVG_B);
Tanja2211 41:d86fced533a8 131 pc.printf("a4\n");
s1340735 10:9319e872c752 132 //naar HID Scope
Tanja2211 6:ad6d4bd69205 133 scope.set(2,emg_valueB); //ruwe data
Tanja2211 6:ad6d4bd69205 134 scope.set(3,filtered_emgB); //filtered
Tanja2211 5:3232d78cb7b3 135 scope.send();
s1340735 2:a86b09b00008 136 }
s1340735 0:abe0bc5c43b7 137
s1340735 19:fdebe6892633 138
s1340735 32:36bb6af01689 139 // *** TRICEPS EN BICEPS CALIBRATIE ***
s1340735 32:36bb6af01689 140 void Calibratie_Triceps()
Tanja2211 9:a1890454e5a7 141 {
s1340735 32:36bb6af01689 142 //triceps drempelwaarde calibreren
s1340735 10:9319e872c752 143
s1340735 32:36bb6af01689 144 Ticker log_timerT;
s1340735 21:f0357a41f43e 145
s1340735 19:fdebe6892633 146 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 9:a1890454e5a7 147 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 9:a1890454e5a7 148 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 2:a86b09b00008 149
s1340735 32:36bb6af01689 150 log_timerT.attach(Triceps, 0.005);
s1340735 32:36bb6af01689 151 wait(3);
s1340735 32:36bb6af01689 152 log_timerT.detach();
s1340735 32:36bb6af01689 153 }
s1340735 10:9319e872c752 154
s1340735 32:36bb6af01689 155 void Calibratie_Biceps()
s1340735 32:36bb6af01689 156 {
s1340735 32:36bb6af01689 157 Ticker log_timerB;
Tanja2211 41:d86fced533a8 158 pc.printf("d\n");
Tanja2211 42:6430dd3a8ea2 159 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
Tanja2211 42:6430dd3a8ea2 160 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
Tanja2211 42:6430dd3a8ea2 161 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
Tanja2211 41:d86fced533a8 162 pc.printf("e\n");
Tanja2211 42:6430dd3a8ea2 163 log_timerB.attach(Biceps, 0.005);
Tanja2211 41:d86fced533a8 164 pc.printf("f\n");
Tanja2211 41:d86fced533a8 165 wait(3);
Tanja2211 41:d86fced533a8 166 pc.printf("g\n");
s1340735 32:36bb6af01689 167 log_timerB.detach();
s1340735 32:36bb6af01689 168 }
s1340735 32:36bb6af01689 169
s1340735 32:36bb6af01689 170 // *** MAIN ***
s1340735 32:36bb6af01689 171 int main()
s1340735 32:36bb6af01689 172 {
s1340735 32:36bb6af01689 173 pc.baud(115200);
s1340735 32:36bb6af01689 174
s1340735 34:514440ddca9a 175 drempelwaardeT=0;
s1340735 34:514440ddca9a 176 drempelwaardeB1=0;
s1340735 34:514440ddca9a 177 drempelwaardeB2=0;
s1340735 34:514440ddca9a 178 drempelwaardeB3=0;
s1340735 35:e60f09b575d7 179
s1340735 36:ccc901c169b7 180 TouchButton TButton;
s1340735 36:ccc901c169b7 181
s1340735 37:8b15c29445d3 182 int key=0;//vraagt om calibratie
Tanja2211 41:d86fced533a8 183
s1340735 32:36bb6af01689 184
s1340735 39:012ff1795e6b 185 pc.printf("key 1 calibratie triceps\n");
s1340735 39:012ff1795e6b 186 pc.printf("key 2 caliratie biceps\n");
s1340735 39:012ff1795e6b 187 pc.printf("key 3 START\n");
Tanja2211 26:e43cae7186f5 188
Tanja2211 41:d86fced533a8 189
s1340735 40:42735d766db1 190 while(true) {
Tanja2211 41:d86fced533a8 191 key = TButton.PressedButton();
s1340735 40:42735d766db1 192 if (key==1) {
Tanja2211 38:017fd93e9efe 193
s1340735 40:42735d766db1 194 //rood
s1340735 40:42735d766db1 195 myled1 = 0;
s1340735 40:42735d766db1 196 myled2 = 1;
s1340735 40:42735d766db1 197 myled3 = 1;
Tanja2211 38:017fd93e9efe 198
s1340735 40:42735d766db1 199 pc.printf("calibratie tricep aan\n");
s1340735 40:42735d766db1 200 wait(2);
Tanja2211 38:017fd93e9efe 201
s1340735 40:42735d766db1 202 Calibratie_Triceps();
s1340735 40:42735d766db1 203 drempelwaardeT=MOVAVG_T-1;
s1340735 40:42735d766db1 204 pc.printf("drempelwaarde triceps is %f\r\n", drempelwaardeT);
s1340735 40:42735d766db1 205 pc.printf("calibratie tricep klaar,\n");
s1340735 40:42735d766db1 206 wait(5);
s1340735 40:42735d766db1 207 }
Tanja2211 38:017fd93e9efe 208
s1340735 40:42735d766db1 209 if (key==2) {
s1340735 39:012ff1795e6b 210
s1340735 40:42735d766db1 211 //green
s1340735 37:8b15c29445d3 212 myled1 = 1;
s1340735 40:42735d766db1 213 myled2 = 0;
s1340735 40:42735d766db1 214 myled3 = 1;
Tanja2211 38:017fd93e9efe 215
s1340735 40:42735d766db1 216 pc.printf("calibratie bicep snelheid 1 aan\n");
s1340735 40:42735d766db1 217 wait(2);
Tanja2211 41:d86fced533a8 218 pc.printf("a\n");
s1340735 40:42735d766db1 219 Calibratie_Biceps();
Tanja2211 41:d86fced533a8 220 pc.printf("b\n");
s1340735 40:42735d766db1 221 drempelwaardeB1=MOVAVG_B-1;
Tanja2211 41:d86fced533a8 222 pc.printf("c\n");
Tanja2211 41:d86fced533a8 223
s1340735 40:42735d766db1 224 pc.printf("drempelwaarde snelheid 1 is %f\r\n", drempelwaardeB1);
s1340735 40:42735d766db1 225 wait(5);
s1340735 36:ccc901c169b7 226
s1340735 40:42735d766db1 227 pc.printf("calibratie bicep snelheid 2 aan\n");
s1340735 40:42735d766db1 228 wait(2);
s1340735 37:8b15c29445d3 229
s1340735 40:42735d766db1 230 Calibratie_Biceps();
s1340735 40:42735d766db1 231 drempelwaardeB2=MOVAVG_B-1;
s1340735 40:42735d766db1 232 pc.printf("drempelwaarde snelheid 2 is %f\r\n", drempelwaardeB2);
s1340735 37:8b15c29445d3 233 wait(5);
s1340735 37:8b15c29445d3 234
s1340735 40:42735d766db1 235 pc.printf("calibratie bicep snelheid 2 aan\n");
s1340735 40:42735d766db1 236 wait(2);
s1340735 35:e60f09b575d7 237
s1340735 40:42735d766db1 238 Calibratie_Biceps();
s1340735 40:42735d766db1 239 wait(2);
s1340735 37:8b15c29445d3 240
s1340735 40:42735d766db1 241 drempelwaardeB2=MOVAVG_B-1;
s1340735 40:42735d766db1 242 pc.printf("drempelwaarde snelheid 3 is %f\r\n", drempelwaardeB3);
s1340735 40:42735d766db1 243
s1340735 40:42735d766db1 244 pc.printf("caliratie biceps is klaar\n");
s1340735 36:ccc901c169b7 245
s1340735 37:8b15c29445d3 246 wait(5);
s1340735 40:42735d766db1 247 }
s1340735 40:42735d766db1 248 if (key==3) {
s1340735 40:42735d766db1 249 if(drempelwaardeT==0) {
s1340735 40:42735d766db1 250 pc.printf("voer calibratie triceps uit\n");
s1340735 40:42735d766db1 251 }
s1340735 40:42735d766db1 252 if (drempelwaardeB1==0) {
s1340735 40:42735d766db1 253 pc.printf("voer calibratie biceps uit\n");
s1340735 40:42735d766db1 254 }
s1340735 40:42735d766db1 255 if (drempelwaardeB2==0) {
s1340735 40:42735d766db1 256 pc.printf("voer calibratie biceps uit\n");
s1340735 40:42735d766db1 257 }
s1340735 40:42735d766db1 258 if (drempelwaardeB3==0) {
s1340735 40:42735d766db1 259 pc.printf("voer calibratie biceps uit\n");
s1340735 40:42735d766db1 260 } else {
s1340735 35:e60f09b575d7 261
s1340735 40:42735d766db1 262 //blue
s1340735 40:42735d766db1 263 myled1 = 1;
s1340735 40:42735d766db1 264 myled2 = 1;
s1340735 40:42735d766db1 265 myled3 = 0;
s1340735 40:42735d766db1 266 wait(0.1);
s1340735 40:42735d766db1 267
s1340735 40:42735d766db1 268 pc.printf("eerst positie dan snelheid aangeven/n");
s1340735 40:42735d766db1 269
s1340735 40:42735d766db1 270 //bepaling van positie met triceps 1
s1340735 40:42735d766db1 271 Ticker log_timerT1;
s1340735 37:8b15c29445d3 272
s1340735 40:42735d766db1 273 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
s1340735 40:42735d766db1 274 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
s1340735 40:42735d766db1 275 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 40:42735d766db1 276
s1340735 40:42735d766db1 277 log_timerT1.attach(Triceps, 0.005);
s1340735 40:42735d766db1 278 wait(10);
s1340735 40:42735d766db1 279 log_timerT1.detach();
s1340735 40:42735d766db1 280
s1340735 40:42735d766db1 281 MOVAVG_T=MOVAVG_Positie1;
s1340735 40:42735d766db1 282
s1340735 40:42735d766db1 283 // positie van batje met behulp van Triceps
s1340735 37:8b15c29445d3 284
s1340735 40:42735d766db1 285 if (MOVAVG_Positie1>= drempelwaardeT) {
s1340735 40:42735d766db1 286 yT1=1;
s1340735 40:42735d766db1 287 } else {
s1340735 40:42735d766db1 288 yT1=0;
s1340735 40:42735d766db1 289 }
s1340735 40:42735d766db1 290
s1340735 40:42735d766db1 291 pc.printf("Triceps eerste meting is klaar.\n");
s1340735 40:42735d766db1 292 wait(5);
s1340735 40:42735d766db1 293
s1340735 40:42735d766db1 294 //bepaling van positie met tricep 2
s1340735 40:42735d766db1 295 Ticker log_timerT2;
s1340735 40:42735d766db1 296
s1340735 40:42735d766db1 297 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
s1340735 40:42735d766db1 298 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
s1340735 40:42735d766db1 299 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 37:8b15c29445d3 300
s1340735 40:42735d766db1 301 log_timerT2.attach(Triceps, 0.005);
s1340735 40:42735d766db1 302 wait(5);
s1340735 40:42735d766db1 303 log_timerT2.detach();
s1340735 40:42735d766db1 304
s1340735 40:42735d766db1 305 MOVAVG_T=MOVAVG_Positie2;
s1340735 40:42735d766db1 306
s1340735 40:42735d766db1 307 if (MOVAVG_Positie2 >= drempelwaardeT) {
s1340735 40:42735d766db1 308 yT2=1;
s1340735 40:42735d766db1 309 } else {
s1340735 40:42735d766db1 310 yT2=0;
s1340735 40:42735d766db1 311 }
s1340735 40:42735d766db1 312
s1340735 40:42735d766db1 313 pc.printf("Triceps tweede meting is klaar.\n");
s1340735 40:42735d766db1 314
s1340735 40:42735d766db1 315 //*** INPUT MOTOR 2 ***
s1340735 40:42735d766db1 316 positie=yT1+yT2;
s1340735 37:8b15c29445d3 317
s1340735 40:42735d766db1 318 //controle positie op scherm
s1340735 40:42735d766db1 319 if (positie==0) {
s1340735 40:42735d766db1 320 pc.printf("Motor 2 blijft op stand 1\n");
s1340735 40:42735d766db1 321 } else {
s1340735 40:42735d766db1 322 if (positie==1) {
s1340735 40:42735d766db1 323 pc.printf("Motor 2 gaat naar stand 2\n");
s1340735 37:8b15c29445d3 324 } else {
s1340735 40:42735d766db1 325 if (positie==2) {
s1340735 40:42735d766db1 326 pc.printf("Motor 2 gaat naar stand 3\n");
s1340735 40:42735d766db1 327 }
s1340735 40:42735d766db1 328 }
s1340735 40:42735d766db1 329 }
s1340735 40:42735d766db1 330
s1340735 40:42735d766db1 331 wait(5);
s1340735 40:42735d766db1 332
s1340735 40:42735d766db1 333 Ticker log_timerB;
s1340735 40:42735d766db1 334
s1340735 40:42735d766db1 335 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
s1340735 40:42735d766db1 336 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
s1340735 40:42735d766db1 337 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
s1340735 40:42735d766db1 338
s1340735 40:42735d766db1 339 log_timerB.attach(Biceps,0.005);
s1340735 40:42735d766db1 340 wait(5);
s1340735 40:42735d766db1 341 log_timerB.detach();
s1340735 40:42735d766db1 342
s1340735 40:42735d766db1 343 //bepaling van snelheidsstand met biceps
s1340735 40:42735d766db1 344
s1340735 40:42735d766db1 345 if (MOVAVG_B >= drempelwaardeB1) {
s1340735 40:42735d766db1 346 yB1=1;
s1340735 40:42735d766db1 347 if (MOVAVG_B >= drempelwaardeB2) {
s1340735 40:42735d766db1 348 yB2=1;
s1340735 40:42735d766db1 349 if (MOVAVG_B >= drempelwaardeB3) {
s1340735 40:42735d766db1 350 yB3=1;
s1340735 40:42735d766db1 351 } else {
s1340735 40:42735d766db1 352 yB3=0;
s1340735 40:42735d766db1 353 }
s1340735 40:42735d766db1 354 } else {
s1340735 40:42735d766db1 355 yB2=0;
s1340735 37:8b15c29445d3 356 }
s1340735 37:8b15c29445d3 357 } else {
s1340735 40:42735d766db1 358 yB1=0;
s1340735 37:8b15c29445d3 359 }
s1340735 36:ccc901c169b7 360
s1340735 40:42735d766db1 361 //*** INPUT MOTOR 1 ***
s1340735 40:42735d766db1 362 snelheidsstand=yB1+yB2+yB3;
s1340735 32:36bb6af01689 363
s1340735 40:42735d766db1 364 //controle snelheidsstand op scherm
s1340735 40:42735d766db1 365 if (snelheidsstand==0) {
s1340735 40:42735d766db1 366 pc.printf("Motor 1 beweegt niet\n");
s1340735 37:8b15c29445d3 367 } else {
s1340735 40:42735d766db1 368 if (snelheidsstand==1) {
s1340735 40:42735d766db1 369 pc.printf("Motor 1 beweegt met snelheid 1\n");
s1340735 37:8b15c29445d3 370 } else {
s1340735 40:42735d766db1 371 if (snelheidsstand==2) {
s1340735 40:42735d766db1 372 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 40:42735d766db1 373 } else {
s1340735 40:42735d766db1 374 if (snelheidsstand==3) {
s1340735 40:42735d766db1 375 pc.printf("Motor 1 beweegt met snelheid 3\n");
s1340735 40:42735d766db1 376 }
s1340735 32:36bb6af01689 377 }
s1340735 32:36bb6af01689 378 }
s1340735 32:36bb6af01689 379 }
s1340735 32:36bb6af01689 380 }
s1340735 35:e60f09b575d7 381 }
s1340735 40:42735d766db1 382
s1340735 36:ccc901c169b7 383 }
s1340735 40:42735d766db1 384 }