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