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@35:e60f09b575d7, 2014-10-28 (annotated)

Committer:
s1340735
Date:
Tue Oct 28 11:11:56 2014 +0000
Revision:
35:e60f09b575d7
Parent:
34:514440ddca9a
Child:
36:ccc901c169b7
while loop er uit gehaald

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 32:36bb6af01689 20 *************************
s1340735 32:36bb6af01689 21 * * * * *
s1340735 32:36bb6af01689 22 * 1 * 2 * 3 * 4 *
s1340735 32:36bb6af01689 23 * * * * *
s1340735 32:36bb6af01689 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 34:514440ddca9a 28 * key 4 will light White LED (R+G+B) -> ?ONBEKEND
s1340735 32:36bb6af01689 29 */
s1340735 0:abe0bc5c43b7 30
s1340735 0:abe0bc5c43b7 31 //*** OBJECTS ***
s1340735 2:a86b09b00008 32 //bicep
s1340735 33:bc0abe29bad1 33 uint16_t emg_valueB;
s1340735 33:bc0abe29bad1 34 float emg_value_f32B;
s1340735 33:bc0abe29bad1 35 float filtered_emgB;
s1340735 0:abe0bc5c43b7 36 float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
s1340735 0:abe0bc5c43b7 37 int yB1, yB2, yB3;
s1340735 21:f0357a41f43e 38 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 39 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 40 int snelheidsstand;
s1340735 2:a86b09b00008 41 //tricep
s1340735 33:bc0abe29bad1 42 uint16_t emg_valueT;
s1340735 33:bc0abe29bad1 43 float emg_value_f32T;
s1340735 33:bc0abe29bad1 44 float filtered_emgT;
s1340735 14:bebdb2ac5ec6 45 float drempelwaardeT;
s1340735 2:a86b09b00008 46 int yT1, yT2;
Tanja2211 15:e779bfbeb8ea 47 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 48 float MOVAVG_Positie1, MOVAVG_Positie2;
s1340735 2:a86b09b00008 49 int positie;
s1340735 2:a86b09b00008 50
s1340735 0:abe0bc5c43b7 51
s1340735 0:abe0bc5c43b7 52 //*** FILTERS ***
s1340735 19:fdebe6892633 53 arm_biquad_casd_df1_inst_f32 notchT;
s1340735 19:fdebe6892633 54 arm_biquad_casd_df1_inst_f32 notchB;
s1340735 21:f0357a41f43e 55 //constants for 50Hz
s1340735 21:f0357a41f43e 56 float notch_const[]= {0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122}; //{a0 a1 a2 -b1 -b2}
s1340735 19:fdebe6892633 57 float notch_states[4];
s1340735 19:fdebe6892633 58
Tanja2211 9:a1890454e5a7 59 arm_biquad_casd_df1_inst_f32 lowpassT;
Tanja2211 9:a1890454e5a7 60 arm_biquad_casd_df1_inst_f32 lowpassB;
s1340735 19:fdebe6892633 61 //constants for 60Hz lowpass
s1340735 19:fdebe6892633 62 float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator
s1340735 0:abe0bc5c43b7 63 float lowpass_states[4];
s1340735 0:abe0bc5c43b7 64
Tanja2211 9:a1890454e5a7 65 arm_biquad_casd_df1_inst_f32 highpassT;
Tanja2211 9:a1890454e5a7 66 arm_biquad_casd_df1_inst_f32 highpassB;
s1340735 19:fdebe6892633 67 //constants for 20Hz highpass
Tanja2211 17:5fd768d0504f 68 float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2}
s1340735 0:abe0bc5c43b7 69 float highpass_states[4];
s1340735 0:abe0bc5c43b7 70
Tanja2211 9:a1890454e5a7 71
Tanja2211 9:a1890454e5a7 72 // *** TRICEPS en BICEPS EMG ***
s1340735 34:514440ddca9a 73
Tanja2211 9:a1890454e5a7 74 void Triceps()
Tanja2211 9:a1890454e5a7 75 {
Tanja2211 9:a1890454e5a7 76 //Triceps lezen
Tanja2211 9:a1890454e5a7 77 emg_valueT = emgT.read_u16();
Tanja2211 9:a1890454e5a7 78 emg_value_f32T = emgT.read();
Tanja2211 9:a1890454e5a7 79
Tanja2211 9:a1890454e5a7 80 //Triceps filteren
Tanja2211 20:5cd52a0daab0 81 arm_biquad_cascade_df1_f32(&notchT, &emg_value_f32T, &filtered_emgT, 1);
s1340735 19:fdebe6892633 82 arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
Tanja2211 9:a1890454e5a7 83 filtered_emgT = fabs(filtered_emgT);
Tanja2211 17:5fd768d0504f 84 arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 );
Tanja2211 20:5cd52a0daab0 85 filtered_emgT = fabs(filtered_emgT);
s1340735 21:f0357a41f43e 86
Tanja2211 9:a1890454e5a7 87 //Triceps moving average
Tanja2211 28:c6b19fdfba01 88 T0=filtered_emgT*100;
Tanja2211 15:e779bfbeb8ea 89 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 90 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 91 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 92 T5=T4, T4=T3, T3=T2, T2=T1, T1=T0;
s1340735 21:f0357a41f43e 93
Tanja2211 16:e997a6fd802a 94 //sturen naar scherm (Realterm)
Tanja2211 16:e997a6fd802a 95 pc.printf("%f\r\n",MOVAVG_T);
Tanja2211 16:e997a6fd802a 96
s1340735 10:9319e872c752 97 //sturen naar HID Scope
Tanja2211 9:a1890454e5a7 98 scope.set(0,emg_valueT); //ruwe data
Tanja2211 9:a1890454e5a7 99 scope.set(1,filtered_emgT); //filtered
Tanja2211 9:a1890454e5a7 100 scope.send();
Tanja2211 9:a1890454e5a7 101 }
Tanja2211 9:a1890454e5a7 102
s1340735 0:abe0bc5c43b7 103 void Biceps()
s1340735 0:abe0bc5c43b7 104 {
Tanja2211 9:a1890454e5a7 105 //Biceps lezen
s1340735 0:abe0bc5c43b7 106 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 107 emg_value_f32B = emgB.read();
s1340735 0:abe0bc5c43b7 108
Tanja2211 9:a1890454e5a7 109 //Biceps filteren
s1340735 19:fdebe6892633 110 arm_biquad_cascade_df1_f32(&notchB, &emg_value_f32B, &filtered_emgB, 1 );
s1340735 19:fdebe6892633 111 arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
s1340735 0:abe0bc5c43b7 112 filtered_emgB = fabs(filtered_emgB);
Tanja2211 17:5fd768d0504f 113 arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 );
Tanja2211 20:5cd52a0daab0 114 filtered_emgB = fabs(filtered_emgB);
s1340735 21:f0357a41f43e 115
Tanja2211 9:a1890454e5a7 116 //Biceps moving average
Tanja2211 22:0be5dfa04f75 117 B0=filtered_emgB*1000;
s1340735 21:f0357a41f43e 118 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 119 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 120 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 121 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 122 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 123 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 124 B3=B2, B2=B1, B1=B0;
s1340735 0:abe0bc5c43b7 125
s1340735 32:36bb6af01689 126 //sturen naar scherm
Tanja2211 16:e997a6fd802a 127 pc.printf("%f\r\n",MOVAVG_B);
Tanja2211 16:e997a6fd802a 128
s1340735 10:9319e872c752 129 //naar HID Scope
Tanja2211 6:ad6d4bd69205 130 scope.set(2,emg_valueB); //ruwe data
Tanja2211 6:ad6d4bd69205 131 scope.set(3,filtered_emgB); //filtered
Tanja2211 5:3232d78cb7b3 132 scope.send();
s1340735 2:a86b09b00008 133 }
s1340735 0:abe0bc5c43b7 134
s1340735 19:fdebe6892633 135
s1340735 32:36bb6af01689 136 // *** TRICEPS EN BICEPS CALIBRATIE ***
s1340735 32:36bb6af01689 137 void Calibratie_Triceps()
Tanja2211 9:a1890454e5a7 138 {
s1340735 32:36bb6af01689 139 //triceps drempelwaarde calibreren
s1340735 10:9319e872c752 140
s1340735 32:36bb6af01689 141 Ticker log_timerT;
s1340735 21:f0357a41f43e 142
s1340735 19:fdebe6892633 143 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 9:a1890454e5a7 144 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 9:a1890454e5a7 145 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 2:a86b09b00008 146
s1340735 32:36bb6af01689 147 log_timerT.attach(Triceps, 0.005);
s1340735 32:36bb6af01689 148 wait(3);
s1340735 32:36bb6af01689 149 log_timerT.detach();
s1340735 32:36bb6af01689 150 }
s1340735 10:9319e872c752 151
s1340735 32:36bb6af01689 152 void Calibratie_Biceps()
s1340735 32:36bb6af01689 153 {
s1340735 32:36bb6af01689 154 Ticker log_timerB;
Tanja2211 26:e43cae7186f5 155
Tanja2211 26:e43cae7186f5 156 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 26:e43cae7186f5 157 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 26:e43cae7186f5 158 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
Tanja2211 26:e43cae7186f5 159
s1340735 32:36bb6af01689 160 log_timerB.attach(Biceps, 0.005);
s1340735 34:514440ddca9a 161 wait(5);
s1340735 32:36bb6af01689 162 log_timerB.detach();
s1340735 32:36bb6af01689 163 }
s1340735 32:36bb6af01689 164
s1340735 32:36bb6af01689 165 // *** MAIN ***
s1340735 32:36bb6af01689 166 int main()
s1340735 32:36bb6af01689 167 {
s1340735 32:36bb6af01689 168 pc.baud(115200);
s1340735 32:36bb6af01689 169
s1340735 32:36bb6af01689 170 TouchButton TButton;
s1340735 32:36bb6af01689 171
s1340735 35:e60f09b575d7 172 int key;
s1340735 32:36bb6af01689 173
s1340735 34:514440ddca9a 174 drempelwaardeT=0;
s1340735 34:514440ddca9a 175 drempelwaardeB1=0;
s1340735 34:514440ddca9a 176 drempelwaardeB2=0;
s1340735 34:514440ddca9a 177 drempelwaardeB3=0;
s1340735 35:e60f09b575d7 178
s1340735 32:36bb6af01689 179 key = TButton.PresedButton();
s1340735 32:36bb6af01689 180
s1340735 32:36bb6af01689 181 pc.printf("key 1 calibratie triceps/n");
s1340735 32:36bb6af01689 182 pc.printf("key 2 caliratie biceps/n");
s1340735 32:36bb6af01689 183 pc.printf("key 3 START/n");
Tanja2211 26:e43cae7186f5 184
s1340735 35:e60f09b575d7 185 if (key==0) {
s1340735 35:e60f09b575d7 186 myled1 = 1;
s1340735 35:e60f09b575d7 187 myled2 = 1;
s1340735 35:e60f09b575d7 188 myled3 = 1;
s1340735 32:36bb6af01689 189
s1340735 35:e60f09b575d7 190 pc.printf("calibratie tricep aan\n");
s1340735 35:e60f09b575d7 191 Calibratie_Triceps();
s1340735 35:e60f09b575d7 192 drempelwaardeT=MOVAVG_T-1;
s1340735 35:e60f09b575d7 193 pc.printf("drempelwaarde triceps is %f\r\n", drempelwaardeT);
s1340735 35:e60f09b575d7 194 pc.printf("calibratie tricep klaar\n");
s1340735 35:e60f09b575d7 195 wait(5);
s1340735 35:e60f09b575d7 196 }
s1340735 35:e60f09b575d7 197
s1340735 35:e60f09b575d7 198 if (key==1) {
s1340735 35:e60f09b575d7 199 myled1 = 1;
s1340735 35:e60f09b575d7 200 myled2 = 1;
s1340735 35:e60f09b575d7 201 myled3 = 0;
s1340735 35:e60f09b575d7 202 wait(0.1);
s1340735 35:e60f09b575d7 203
s1340735 35:e60f09b575d7 204 pc.printf("calibratie bicep snelheid 1 aan\n");
s1340735 35:e60f09b575d7 205 Calibratie_Biceps();
s1340735 35:e60f09b575d7 206 drempelwaardeB1=MOVAVG_B-1;
s1340735 35:e60f09b575d7 207 pc.printf("drempelwaarde snelheid 1 is %f\r\n", drempelwaardeB1);
s1340735 35:e60f09b575d7 208 wait(5);
s1340735 32:36bb6af01689 209
s1340735 35:e60f09b575d7 210 pc.printf("calibratie bicep snelheid 2 aan\n");
s1340735 35:e60f09b575d7 211 Calibratie_Biceps();
s1340735 35:e60f09b575d7 212 drempelwaardeB2=MOVAVG_B-1;
s1340735 35:e60f09b575d7 213 pc.printf("drempelwaarde snelheid 2 is %f\r\n", drempelwaardeB2);
s1340735 35:e60f09b575d7 214 wait(5);
s1340735 35:e60f09b575d7 215
s1340735 35:e60f09b575d7 216 pc.printf("calibratie bicep snelheid 2 aan\n");
s1340735 35:e60f09b575d7 217 Calibratie_Biceps();
s1340735 35:e60f09b575d7 218 drempelwaardeB2=MOVAVG_B-1;
s1340735 35:e60f09b575d7 219 pc.printf("drempelwaarde snelheid 3 is %f\r\n", drempelwaardeB3);
s1340735 35:e60f09b575d7 220
s1340735 35:e60f09b575d7 221 pc.printf("caliratie biceps is klaar/n");
s1340735 32:36bb6af01689 222
s1340735 35:e60f09b575d7 223 wait(5);
s1340735 35:e60f09b575d7 224 }
s1340735 32:36bb6af01689 225
s1340735 35:e60f09b575d7 226 if (key==2) {
s1340735 35:e60f09b575d7 227 if (drempelwaardeT==0) {
s1340735 35:e60f09b575d7 228 pc.printf("voer calibratie triceps uit");
s1340735 35:e60f09b575d7 229 } else if (drempelwaardeB1==0) {
s1340735 35:e60f09b575d7 230 pc.printf("voer calibratie biceps uit");
s1340735 35:e60f09b575d7 231 } else if (drempelwaardeB2==0) {
s1340735 35:e60f09b575d7 232 pc.printf("voer calibratie biceps uit");
s1340735 35:e60f09b575d7 233 } else if (drempelwaardeB3==0) {
s1340735 35:e60f09b575d7 234 pc.printf("voer calibratie biceps uit");
s1340735 35:e60f09b575d7 235 } else {
s1340735 32:36bb6af01689 236 myled1 = 1;
s1340735 35:e60f09b575d7 237 myled2 = 0;
s1340735 35:e60f09b575d7 238 myled3 = 1;
s1340735 32:36bb6af01689 239 wait(0.1);
Tanja2211 26:e43cae7186f5 240
s1340735 35:e60f09b575d7 241 pc.printf("eerst positie dan snelheid aangeven/n");
s1340735 35:e60f09b575d7 242
s1340735 35:e60f09b575d7 243 //bepaling van positie met triceps 1
s1340735 35:e60f09b575d7 244 Ticker log_timerT1;
s1340735 35:e60f09b575d7 245
s1340735 35:e60f09b575d7 246 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
s1340735 35:e60f09b575d7 247 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
s1340735 35:e60f09b575d7 248 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 32:36bb6af01689 249
s1340735 35:e60f09b575d7 250 log_timerT1.attach(Triceps, 0.005);
s1340735 35:e60f09b575d7 251 wait(10);
s1340735 35:e60f09b575d7 252 log_timerT1.detach();
s1340735 35:e60f09b575d7 253
s1340735 35:e60f09b575d7 254 MOVAVG_T=MOVAVG_Positie1;
s1340735 35:e60f09b575d7 255
s1340735 35:e60f09b575d7 256 // positie van batje met behulp van Triceps
s1340735 32:36bb6af01689 257
s1340735 35:e60f09b575d7 258 if (MOVAVG_Positie1>= drempelwaardeT) {
s1340735 35:e60f09b575d7 259 yT1=1;
s1340735 35:e60f09b575d7 260 } else {
s1340735 35:e60f09b575d7 261 yT1=0;
s1340735 35:e60f09b575d7 262 }
s1340735 35:e60f09b575d7 263
s1340735 35:e60f09b575d7 264 pc.printf("Triceps eerste meting is klaar.\n");
s1340735 32:36bb6af01689 265 wait(5);
s1340735 32:36bb6af01689 266
s1340735 35:e60f09b575d7 267 //bepaling van positie met tricep 2
s1340735 35:e60f09b575d7 268 Ticker log_timerT2;
s1340735 29:52a0d241e1b0 269
s1340735 35:e60f09b575d7 270 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
s1340735 35:e60f09b575d7 271 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
s1340735 35:e60f09b575d7 272 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 35:e60f09b575d7 273
s1340735 35:e60f09b575d7 274 log_timerT2.attach(Triceps, 0.005);
s1340735 35:e60f09b575d7 275 wait(5);
s1340735 35:e60f09b575d7 276 log_timerT2.detach();
s1340735 35:e60f09b575d7 277
s1340735 35:e60f09b575d7 278 MOVAVG_T=MOVAVG_Positie2;
s1340735 32:36bb6af01689 279
s1340735 35:e60f09b575d7 280 if (MOVAVG_Positie2 >= drempelwaardeT) {
s1340735 35:e60f09b575d7 281 yT2=1;
s1340735 35:e60f09b575d7 282 } else {
s1340735 35:e60f09b575d7 283 yT2=0;
s1340735 35:e60f09b575d7 284 }
s1340735 32:36bb6af01689 285
s1340735 35:e60f09b575d7 286 pc.printf("Triceps tweede meting is klaar.\n");
s1340735 32:36bb6af01689 287
s1340735 35:e60f09b575d7 288 //*** INPUT MOTOR 2 ***
s1340735 35:e60f09b575d7 289 positie=yT1+yT2;
s1340735 34:514440ddca9a 290
s1340735 35:e60f09b575d7 291 //controle positie op scherm
s1340735 35:e60f09b575d7 292 if (positie==0) {
s1340735 35:e60f09b575d7 293 pc.printf("Motor 2 blijft op stand 1\n");
s1340735 35:e60f09b575d7 294 } else {
s1340735 35:e60f09b575d7 295 if (positie==1) {
s1340735 35:e60f09b575d7 296 pc.printf("Motor 2 gaat naar stand 2\n");
s1340735 34:514440ddca9a 297 } else {
s1340735 35:e60f09b575d7 298 if (positie==2) {
s1340735 35:e60f09b575d7 299 pc.printf("Motor 2 gaat naar stand 3\n");
s1340735 32:36bb6af01689 300 }
s1340735 32:36bb6af01689 301 }
s1340735 35:e60f09b575d7 302 }
s1340735 32:36bb6af01689 303
s1340735 35:e60f09b575d7 304 wait(5);
s1340735 32:36bb6af01689 305
s1340735 35:e60f09b575d7 306 Ticker log_timerB;
s1340735 0:abe0bc5c43b7 307
s1340735 35:e60f09b575d7 308 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
s1340735 35:e60f09b575d7 309 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
s1340735 35:e60f09b575d7 310 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
s1340735 32:36bb6af01689 311
s1340735 35:e60f09b575d7 312 log_timerB.attach(Biceps,0.005);
s1340735 35:e60f09b575d7 313 wait(5);
s1340735 35:e60f09b575d7 314 log_timerB.detach();
s1340735 35:e60f09b575d7 315
s1340735 35:e60f09b575d7 316 //bepaling van snelheidsstand met biceps
s1340735 32:36bb6af01689 317
s1340735 35:e60f09b575d7 318 if (MOVAVG_B >= drempelwaardeB1) {
s1340735 35:e60f09b575d7 319 yB1=1;
s1340735 35:e60f09b575d7 320 if (MOVAVG_B >= drempelwaardeB2) {
s1340735 35:e60f09b575d7 321 yB2=1;
s1340735 35:e60f09b575d7 322 if (MOVAVG_B >= drempelwaardeB3) {
s1340735 35:e60f09b575d7 323 yB3=1;
s1340735 32:36bb6af01689 324 } else {
s1340735 35:e60f09b575d7 325 yB3=0;
s1340735 32:36bb6af01689 326 }
s1340735 32:36bb6af01689 327 } else {
s1340735 35:e60f09b575d7 328 yB2=0;
s1340735 32:36bb6af01689 329 }
s1340735 35:e60f09b575d7 330 } else {
s1340735 35:e60f09b575d7 331 yB1=0;
s1340735 35:e60f09b575d7 332 }
s1340735 32:36bb6af01689 333
s1340735 35:e60f09b575d7 334 //*** INPUT MOTOR 1 ***
s1340735 35:e60f09b575d7 335 snelheidsstand=yB1+yB2+yB3;
s1340735 32:36bb6af01689 336
s1340735 35:e60f09b575d7 337 //controle snelheidsstand op scherm
s1340735 35:e60f09b575d7 338 if (snelheidsstand==0) {
s1340735 35:e60f09b575d7 339 pc.printf("Motor 1 beweegt niet\n");
s1340735 35:e60f09b575d7 340 } else {
s1340735 35:e60f09b575d7 341 if (snelheidsstand==1) {
s1340735 35:e60f09b575d7 342 pc.printf("Motor 1 beweegt met snelheid 1\n");
s1340735 32:36bb6af01689 343 } else {
s1340735 35:e60f09b575d7 344 if (snelheidsstand==2) {
s1340735 35:e60f09b575d7 345 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 32:36bb6af01689 346 } else {
s1340735 35:e60f09b575d7 347 if (snelheidsstand==3) {
s1340735 35:e60f09b575d7 348 pc.printf("Motor 1 beweegt met snelheid 3\n");
s1340735 32:36bb6af01689 349 }
s1340735 32:36bb6af01689 350 }
s1340735 32:36bb6af01689 351 }
s1340735 32:36bb6af01689 352 }
s1340735 35:e60f09b575d7 353 }
s1340735 35:e60f09b575d7 354 if(key==3) {
s1340735 33:bc0abe29bad1 355 myled1 = 0;
s1340735 33:bc0abe29bad1 356 myled2 = 1;
s1340735 33:bc0abe29bad1 357 myled3 = 1;
s1340735 33:bc0abe29bad1 358 wait(0.1);
s1340735 33:bc0abe29bad1 359
s1340735 33:bc0abe29bad1 360 pc.printf("onbekend\n");
s1340735 29:52a0d241e1b0 361 }
s1340735 2:a86b09b00008 362 }
s1340735 35:e60f09b575d7 363 }