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@48:b354afb564f2, 2014-10-29 (annotated)

Committer:
Tanja2211
Date:
Wed Oct 29 09:33:18 2014 +0000
Revision:
48:b354afb564f2
Parent:
47:3bdc6a55abb6 
AWESOME

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 48:b354afb564f2 87 T0=filtered_emgT*1000;
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)
s1340735 43:bc93d31b80f5 94 pc.printf("Moving average T %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 43:bc93d31b80f5 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 43:bc93d31b80f5 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 43:bc93d31b80f5 124
s1340735 32:36bb6af01689 125 //sturen naar scherm
s1340735 43:bc93d31b80f5 126 pc.printf("Moving average B %f\r\n",MOVAVG_B);
s1340735 43:bc93d31b80f5 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);
Tanja2211 48:b354afb564f2 147 wait(2);
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;
s1340735 43:bc93d31b80f5 154
Tanja2211 42:6430dd3a8ea2 155 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
Tanja2211 42:6430dd3a8ea2 156 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
Tanja2211 42:6430dd3a8ea2 157 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
s1340735 43:bc93d31b80f5 158
Tanja2211 42:6430dd3a8ea2 159 log_timerB.attach(Biceps, 0.005);
Tanja2211 48:b354afb564f2 160 wait(2);
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 45:32ca80fb87e4 176 myled1=1;
s1340735 45:32ca80fb87e4 177 myled2=1;
s1340735 45:32ca80fb87e4 178 myled3=1;
s1340735 45:32ca80fb87e4 179
s1340735 43:bc93d31b80f5 180 int key=0;
s1340735 32:36bb6af01689 181
s1340735 39:012ff1795e6b 182 pc.printf("key 1 calibratie triceps\n");
s1340735 39:012ff1795e6b 183 pc.printf("key 2 caliratie biceps\n");
s1340735 39:012ff1795e6b 184 pc.printf("key 3 START\n");
Tanja2211 26:e43cae7186f5 185
s1340735 40:42735d766db1 186 while(true) {
Tanja2211 41:d86fced533a8 187 key = TButton.PressedButton();
s1340735 44:bcd0bb522bf5 188 if (key==1) {//duurt 8 seconden
s1340735 40:42735d766db1 189 //rood
s1340735 40:42735d766db1 190 myled1 = 0;
s1340735 40:42735d766db1 191 myled2 = 1;
s1340735 40:42735d766db1 192 myled3 = 1;
Tanja2211 38:017fd93e9efe 193
s1340735 40:42735d766db1 194 pc.printf("calibratie tricep aan\n");
s1340735 40:42735d766db1 195 wait(2);
Tanja2211 38:017fd93e9efe 196
s1340735 40:42735d766db1 197 Calibratie_Triceps();
s1340735 40:42735d766db1 198 drempelwaardeT=MOVAVG_T-1;
s1340735 40:42735d766db1 199 pc.printf("drempelwaarde triceps is %f\r\n", drempelwaardeT);
s1340735 43:bc93d31b80f5 200
s1340735 40:42735d766db1 201 pc.printf("calibratie tricep klaar,\n");
Tanja2211 47:3bdc6a55abb6 202
Tanja2211 47:3bdc6a55abb6 203 myled1 = 0;
Tanja2211 47:3bdc6a55abb6 204 myled2 = 0;
Tanja2211 47:3bdc6a55abb6 205 myled3 = 0;
Tanja2211 47:3bdc6a55abb6 206
Tanja2211 47:3bdc6a55abb6 207 wait(2);
s1340735 45:32ca80fb87e4 208
s1340735 45:32ca80fb87e4 209 myled1=1;
s1340735 45:32ca80fb87e4 210 myled2=1;
s1340735 45:32ca80fb87e4 211 myled3=1;
s1340735 40:42735d766db1 212 }
Tanja2211 38:017fd93e9efe 213
s1340735 44:bcd0bb522bf5 214 if (key==2) {//duurt 26 seconden
s1340735 40:42735d766db1 215 //green
s1340735 37:8b15c29445d3 216 myled1 = 1;
s1340735 40:42735d766db1 217 myled2 = 0;
s1340735 40:42735d766db1 218 myled3 = 1;
Tanja2211 38:017fd93e9efe 219
s1340735 40:42735d766db1 220 pc.printf("calibratie bicep snelheid 1 aan\n");
s1340735 40:42735d766db1 221 wait(2);
s1340735 40:42735d766db1 222 Calibratie_Biceps();
s1340735 40:42735d766db1 223 drempelwaardeB1=MOVAVG_B-1;
s1340735 40:42735d766db1 224 pc.printf("drempelwaarde snelheid 1 is %f\r\n", drempelwaardeB1);
Tanja2211 47:3bdc6a55abb6 225
Tanja2211 47:3bdc6a55abb6 226 myled1 = 0;
Tanja2211 47:3bdc6a55abb6 227 myled2 = 0;
Tanja2211 47:3bdc6a55abb6 228 myled3 = 0;
Tanja2211 47:3bdc6a55abb6 229
Tanja2211 47:3bdc6a55abb6 230 wait(2);
Tanja2211 47:3bdc6a55abb6 231
Tanja2211 47:3bdc6a55abb6 232 myled1 = 1;
Tanja2211 47:3bdc6a55abb6 233 myled2 = 0;
Tanja2211 47:3bdc6a55abb6 234 myled3 = 1;
s1340735 37:8b15c29445d3 235
s1340735 44:bcd0bb522bf5 236 pc.printf("calibratie biceps snelheid 2 aan\n");
s1340735 40:42735d766db1 237 wait(2);
s1340735 43:bc93d31b80f5 238 Calibratie_Biceps();
s1340735 43:bc93d31b80f5 239 drempelwaardeB2=MOVAVG_B-1;
s1340735 43:bc93d31b80f5 240 pc.printf("drempelwaarde snelheid 2 is %f\r\n", drempelwaardeB2);
Tanja2211 47:3bdc6a55abb6 241
Tanja2211 47:3bdc6a55abb6 242 myled1 = 0;
Tanja2211 47:3bdc6a55abb6 243 myled2 = 0;
Tanja2211 47:3bdc6a55abb6 244 myled3 = 0;
Tanja2211 47:3bdc6a55abb6 245
Tanja2211 47:3bdc6a55abb6 246 wait(2);
Tanja2211 47:3bdc6a55abb6 247
Tanja2211 47:3bdc6a55abb6 248 myled1 = 1;
Tanja2211 47:3bdc6a55abb6 249 myled2 = 0;
Tanja2211 47:3bdc6a55abb6 250 myled3 = 1;
s1340735 35:e60f09b575d7 251
s1340735 44:bcd0bb522bf5 252 pc.printf("calibratie biceps snelheid 3 aan\n");
s1340735 40:42735d766db1 253 wait(2);
s1340735 43:bc93d31b80f5 254 Calibratie_Biceps();
s1340735 43:bc93d31b80f5 255 drempelwaardeB3=MOVAVG_B-1;
s1340735 40:42735d766db1 256 pc.printf("drempelwaarde snelheid 3 is %f\r\n", drempelwaardeB3);
s1340735 40:42735d766db1 257
Tanja2211 47:3bdc6a55abb6 258 myled1 = 0;
Tanja2211 47:3bdc6a55abb6 259 myled2 = 0;
Tanja2211 47:3bdc6a55abb6 260 myled3 = 0;
Tanja2211 47:3bdc6a55abb6 261
Tanja2211 47:3bdc6a55abb6 262 wait(2);
Tanja2211 47:3bdc6a55abb6 263
s1340735 40:42735d766db1 264 pc.printf("caliratie biceps is klaar\n");
Tanja2211 47:3bdc6a55abb6 265 wait(3);
s1340735 45:32ca80fb87e4 266
s1340735 45:32ca80fb87e4 267 myled1=1;
s1340735 45:32ca80fb87e4 268 myled2=1;
s1340735 45:32ca80fb87e4 269 myled3=1;
s1340735 40:42735d766db1 270 }
s1340735 43:bc93d31b80f5 271
s1340735 44:bcd0bb522bf5 272 if (key==3) {//duurt 34 seconden
s1340735 44:bcd0bb522bf5 273
s1340735 44:bcd0bb522bf5 274 //blue
s1340735 44:bcd0bb522bf5 275 myled1 = 1;
s1340735 44:bcd0bb522bf5 276 myled2 = 1;
s1340735 44:bcd0bb522bf5 277 myled3 = 0;
Tanja2211 47:3bdc6a55abb6 278 wait(3);
s1340735 44:bcd0bb522bf5 279
s1340735 43:bc93d31b80f5 280 if(drempelwaardeT==0) {
s1340735 43:bc93d31b80f5 281 pc.printf("geen waarde calibratie TRICEPS \n");
s1340735 46:24e350229ce8 282 myled1 = 0;
s1340735 46:24e350229ce8 283 myled2 = 0;
s1340735 46:24e350229ce8 284 myled3 = 0;
s1340735 43:bc93d31b80f5 285 }
s1340735 43:bc93d31b80f5 286 if (drempelwaardeB1==0) {
s1340735 43:bc93d31b80f5 287 pc.printf("geen waarde calibratie BICEPS 1 \n");
s1340735 46:24e350229ce8 288 myled1 = 0;
s1340735 46:24e350229ce8 289 myled2 = 0;
s1340735 46:24e350229ce8 290 myled3 = 0;
s1340735 43:bc93d31b80f5 291 }
s1340735 43:bc93d31b80f5 292 if (drempelwaardeB2==0) {
s1340735 43:bc93d31b80f5 293 pc.printf("geen waarde calibratie BICEPS 2 \n");
s1340735 46:24e350229ce8 294 myled1 = 0;
s1340735 46:24e350229ce8 295 myled2 = 0;
s1340735 46:24e350229ce8 296 myled3 = 0;
s1340735 43:bc93d31b80f5 297 }
s1340735 43:bc93d31b80f5 298 if (drempelwaardeB3==0) {
s1340735 43:bc93d31b80f5 299 pc.printf("geen waarde calibratie BICEPS 3 \n");
s1340735 46:24e350229ce8 300 myled1 = 0;
s1340735 46:24e350229ce8 301 myled2 = 0;
s1340735 46:24e350229ce8 302 myled3 = 0;
s1340735 43:bc93d31b80f5 303 } else {
s1340735 44:bcd0bb522bf5 304
s1340735 43:bc93d31b80f5 305 pc.printf("eerst positie dan snelheid aangeven /n");
s1340735 40:42735d766db1 306
s1340735 40:42735d766db1 307 //bepaling van positie met triceps 1
s1340735 40:42735d766db1 308 Ticker log_timerT1;
s1340735 37:8b15c29445d3 309
s1340735 40:42735d766db1 310 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
s1340735 40:42735d766db1 311 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
s1340735 40:42735d766db1 312 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 40:42735d766db1 313
Tanja2211 47:3bdc6a55abb6 314 myled1 = 0;
Tanja2211 47:3bdc6a55abb6 315 myled2 = 1;
Tanja2211 47:3bdc6a55abb6 316 myled3 = 1;
Tanja2211 47:3bdc6a55abb6 317
s1340735 40:42735d766db1 318 log_timerT1.attach(Triceps, 0.005);
Tanja2211 48:b354afb564f2 319 wait(3);
s1340735 40:42735d766db1 320 log_timerT1.detach();
s1340735 40:42735d766db1 321
s1340735 40:42735d766db1 322 // positie van batje met behulp van Triceps
s1340735 37:8b15c29445d3 323
Tanja2211 48:b354afb564f2 324 if (MOVAVG_T >= drempelwaardeT) {
s1340735 40:42735d766db1 325 yT1=1;
s1340735 40:42735d766db1 326 } else {
s1340735 40:42735d766db1 327 yT1=0;
s1340735 40:42735d766db1 328 }
s1340735 44:bcd0bb522bf5 329
s1340735 43:bc93d31b80f5 330 pc.printf("Triceps meting 1 is klaar.\n");
Tanja2211 47:3bdc6a55abb6 331 myled1 = 1;
Tanja2211 47:3bdc6a55abb6 332 myled2 = 1;
Tanja2211 47:3bdc6a55abb6 333 myled3 = 0;
s1340735 44:bcd0bb522bf5 334 wait(3);
s1340735 40:42735d766db1 335
Tanja2211 48:b354afb564f2 336 //bepaling van positie met tricep 2
Tanja2211 48:b354afb564f2 337 Ticker log_timerT2;
Tanja2211 48:b354afb564f2 338
Tanja2211 48:b354afb564f2 339 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 48:b354afb564f2 340 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 48:b354afb564f2 341 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
Tanja2211 48:b354afb564f2 342
Tanja2211 48:b354afb564f2 343 myled1 = 0;
Tanja2211 48:b354afb564f2 344 myled2 = 1;
Tanja2211 48:b354afb564f2 345 myled3 = 1;
Tanja2211 48:b354afb564f2 346
Tanja2211 48:b354afb564f2 347 log_timerT2.attach(Triceps, 0.005);
Tanja2211 48:b354afb564f2 348 wait(3);
Tanja2211 48:b354afb564f2 349 log_timerT2.detach();
Tanja2211 48:b354afb564f2 350
Tanja2211 48:b354afb564f2 351 if (MOVAVG_T >= drempelwaardeT) {
Tanja2211 48:b354afb564f2 352 yT2=1;
Tanja2211 48:b354afb564f2 353 } else {
Tanja2211 48:b354afb564f2 354 yT2=0;
Tanja2211 48:b354afb564f2 355 }
Tanja2211 48:b354afb564f2 356
Tanja2211 48:b354afb564f2 357 pc.printf("Triceps meting 2 is klaar.\n");
Tanja2211 48:b354afb564f2 358 myled1 = 1;
Tanja2211 48:b354afb564f2 359 myled2 = 1;
Tanja2211 48:b354afb564f2 360 myled3 = 0;
Tanja2211 48:b354afb564f2 361 wait(3);
Tanja2211 48:b354afb564f2 362
Tanja2211 48:b354afb564f2 363
s1340735 40:42735d766db1 364 //*** INPUT MOTOR 2 ***
s1340735 40:42735d766db1 365 positie=yT1+yT2;
s1340735 37:8b15c29445d3 366
s1340735 40:42735d766db1 367 //controle positie op scherm
s1340735 40:42735d766db1 368 if (positie==0) {
s1340735 40:42735d766db1 369 pc.printf("Motor 2 blijft op stand 1\n");
s1340735 40:42735d766db1 370 } else {
s1340735 40:42735d766db1 371 if (positie==1) {
s1340735 40:42735d766db1 372 pc.printf("Motor 2 gaat naar stand 2\n");
s1340735 37:8b15c29445d3 373 } else {
s1340735 40:42735d766db1 374 if (positie==2) {
s1340735 40:42735d766db1 375 pc.printf("Motor 2 gaat naar stand 3\n");
s1340735 40:42735d766db1 376 }
s1340735 40:42735d766db1 377 }
s1340735 40:42735d766db1 378 }
s1340735 40:42735d766db1 379
Tanja2211 48:b354afb564f2 380 wait(2);
s1340735 40:42735d766db1 381
s1340735 40:42735d766db1 382 Ticker log_timerB;
s1340735 40:42735d766db1 383
s1340735 40:42735d766db1 384 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
s1340735 40:42735d766db1 385 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
s1340735 40:42735d766db1 386 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
s1340735 40:42735d766db1 387
Tanja2211 47:3bdc6a55abb6 388 myled1 = 1;
Tanja2211 47:3bdc6a55abb6 389 myled2 = 0;
Tanja2211 47:3bdc6a55abb6 390 myled3 = 1;
Tanja2211 47:3bdc6a55abb6 391
s1340735 40:42735d766db1 392 log_timerB.attach(Biceps,0.005);
Tanja2211 48:b354afb564f2 393 wait(3);
s1340735 40:42735d766db1 394 log_timerB.detach();
s1340735 40:42735d766db1 395
s1340735 40:42735d766db1 396 //bepaling van snelheidsstand met biceps
s1340735 40:42735d766db1 397
s1340735 40:42735d766db1 398 if (MOVAVG_B >= drempelwaardeB1) {
s1340735 40:42735d766db1 399 yB1=1;
s1340735 40:42735d766db1 400 if (MOVAVG_B >= drempelwaardeB2) {
s1340735 40:42735d766db1 401 yB2=1;
s1340735 40:42735d766db1 402 if (MOVAVG_B >= drempelwaardeB3) {
s1340735 40:42735d766db1 403 yB3=1;
s1340735 40:42735d766db1 404 } else {
s1340735 40:42735d766db1 405 yB3=0;
s1340735 40:42735d766db1 406 }
s1340735 40:42735d766db1 407 } else {
s1340735 40:42735d766db1 408 yB2=0;
s1340735 37:8b15c29445d3 409 }
s1340735 37:8b15c29445d3 410 } else {
s1340735 40:42735d766db1 411 yB1=0;
s1340735 37:8b15c29445d3 412 }
s1340735 44:bcd0bb522bf5 413 pc.printf("Biceps meting is klaar.\n");
Tanja2211 47:3bdc6a55abb6 414 myled1 = 1;
Tanja2211 47:3bdc6a55abb6 415 myled2 = 1;
Tanja2211 47:3bdc6a55abb6 416 myled3 = 0;
s1340735 44:bcd0bb522bf5 417 wait(3);
s1340735 36:ccc901c169b7 418
s1340735 40:42735d766db1 419 //*** INPUT MOTOR 1 ***
s1340735 40:42735d766db1 420 snelheidsstand=yB1+yB2+yB3;
s1340735 32:36bb6af01689 421
s1340735 40:42735d766db1 422 //controle snelheidsstand op scherm
s1340735 40:42735d766db1 423 if (snelheidsstand==0) {
s1340735 40:42735d766db1 424 pc.printf("Motor 1 beweegt niet\n");
s1340735 37:8b15c29445d3 425 } else {
s1340735 40:42735d766db1 426 if (snelheidsstand==1) {
s1340735 40:42735d766db1 427 pc.printf("Motor 1 beweegt met snelheid 1\n");
s1340735 37:8b15c29445d3 428 } else {
s1340735 40:42735d766db1 429 if (snelheidsstand==2) {
s1340735 40:42735d766db1 430 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 40:42735d766db1 431 } else {
s1340735 40:42735d766db1 432 if (snelheidsstand==3) {
s1340735 40:42735d766db1 433 pc.printf("Motor 1 beweegt met snelheid 3\n");
s1340735 40:42735d766db1 434 }
s1340735 32:36bb6af01689 435 }
s1340735 32:36bb6af01689 436 }
s1340735 32:36bb6af01689 437 }
s1340735 44:bcd0bb522bf5 438 wait(5);
s1340735 45:32ca80fb87e4 439
s1340735 45:32ca80fb87e4 440 myled1=1;
s1340735 45:32ca80fb87e4 441 myled2=1;
s1340735 45:32ca80fb87e4 442 myled3=1;
s1340735 32:36bb6af01689 443 }
s1340735 35:e60f09b575d7 444 }
s1340735 40:42735d766db1 445
s1340735 36:ccc901c169b7 446 }
s1340735 44:bcd0bb522bf5 447 }