![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
emg
Dependencies: HIDScope MODSERIAL mbed-dsp mbed TouchButton
Fork of test by
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?
User | Revision | Line number | New 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(¬chT, &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(¬chB, &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(¬chT,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(¬chB,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(¬chT,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(¬chT,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(¬chB,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 | } |