![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
emg
Dependencies: HIDScope MODSERIAL mbed-dsp mbed TouchButton
Fork of test by
main.cpp@21:f0357a41f43e, 2014-10-24 (annotated)
- Committer:
- s1340735
- Date:
- Fri Oct 24 12:46:26 2014 +0000
- Revision:
- 21:f0357a41f43e
- Parent:
- 20:5cd52a0daab0
- Child:
- 22:0be5dfa04f75
moving average bicep 30->60. tweede void tricep in de if loop
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 | 0:abe0bc5c43b7 | 5 | |
s1340735 | 2:a86b09b00008 | 6 | MODSERIAL pc(USBTX,USBRX); |
s1340735 | 2:a86b09b00008 | 7 | |
Tanja2211 | 9:a1890454e5a7 | 8 | HIDScope scope(4); |
s1340735 | 0:abe0bc5c43b7 | 9 | |
s1340735 | 21:f0357a41f43e | 10 | AnalogIn emgB(PTB1); //biceps |
s1340735 | 21:f0357a41f43e | 11 | AnalogIn emgT(PTB2); //tricep |
s1340735 | 21:f0357a41f43e | 12 | |
s1340735 | 0:abe0bc5c43b7 | 13 | |
s1340735 | 0:abe0bc5c43b7 | 14 | //*** OBJECTS *** |
s1340735 | 2:a86b09b00008 | 15 | //bicep |
Tanja2211 | 9:a1890454e5a7 | 16 | uint16_t emg_valueB; |
Tanja2211 | 9:a1890454e5a7 | 17 | float emg_value_f32B; |
s1340735 | 2:a86b09b00008 | 18 | float filtered_emgB; |
s1340735 | 0:abe0bc5c43b7 | 19 | float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3 |
s1340735 | 0:abe0bc5c43b7 | 20 | int yB1, yB2, yB3; |
s1340735 | 21:f0357a41f43e | 21 | 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 | 22 | 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 | 23 | int snelheidsstand; |
s1340735 | 2:a86b09b00008 | 24 | //tricep |
Tanja2211 | 9:a1890454e5a7 | 25 | uint16_t emg_valueT; |
Tanja2211 | 9:a1890454e5a7 | 26 | float emg_value_f32T; |
s1340735 | 2:a86b09b00008 | 27 | float filtered_emgT; |
s1340735 | 14:bebdb2ac5ec6 | 28 | float drempelwaardeT; |
s1340735 | 2:a86b09b00008 | 29 | int yT1, yT2; |
Tanja2211 | 15:e779bfbeb8ea | 30 | 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 | 31 | float MOVAVG_Positie1, MOVAVG_Positie2; |
s1340735 | 2:a86b09b00008 | 32 | int positie; |
s1340735 | 2:a86b09b00008 | 33 | |
s1340735 | 0:abe0bc5c43b7 | 34 | |
s1340735 | 0:abe0bc5c43b7 | 35 | //*** FILTERS *** |
s1340735 | 19:fdebe6892633 | 36 | arm_biquad_casd_df1_inst_f32 notchT; |
s1340735 | 19:fdebe6892633 | 37 | arm_biquad_casd_df1_inst_f32 notchB; |
s1340735 | 21:f0357a41f43e | 38 | //constants for 50Hz |
s1340735 | 21:f0357a41f43e | 39 | float notch_const[]= {0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122}; //{a0 a1 a2 -b1 -b2} |
s1340735 | 19:fdebe6892633 | 40 | float notch_states[4]; |
s1340735 | 19:fdebe6892633 | 41 | |
Tanja2211 | 9:a1890454e5a7 | 42 | arm_biquad_casd_df1_inst_f32 lowpassT; |
Tanja2211 | 9:a1890454e5a7 | 43 | arm_biquad_casd_df1_inst_f32 lowpassB; |
s1340735 | 19:fdebe6892633 | 44 | //constants for 60Hz lowpass |
s1340735 | 19:fdebe6892633 | 45 | float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator |
s1340735 | 0:abe0bc5c43b7 | 46 | float lowpass_states[4]; |
s1340735 | 0:abe0bc5c43b7 | 47 | |
Tanja2211 | 9:a1890454e5a7 | 48 | arm_biquad_casd_df1_inst_f32 highpassT; |
Tanja2211 | 9:a1890454e5a7 | 49 | arm_biquad_casd_df1_inst_f32 highpassB; |
s1340735 | 19:fdebe6892633 | 50 | //constants for 20Hz highpass |
Tanja2211 | 17:5fd768d0504f | 51 | float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2} |
s1340735 | 0:abe0bc5c43b7 | 52 | float highpass_states[4]; |
s1340735 | 0:abe0bc5c43b7 | 53 | |
Tanja2211 | 9:a1890454e5a7 | 54 | |
Tanja2211 | 9:a1890454e5a7 | 55 | // *** TRICEPS en BICEPS EMG *** |
Tanja2211 | 9:a1890454e5a7 | 56 | void Triceps() |
Tanja2211 | 9:a1890454e5a7 | 57 | { |
Tanja2211 | 9:a1890454e5a7 | 58 | //Triceps lezen |
Tanja2211 | 9:a1890454e5a7 | 59 | emg_valueT = emgT.read_u16(); |
Tanja2211 | 9:a1890454e5a7 | 60 | emg_value_f32T = emgT.read(); |
Tanja2211 | 9:a1890454e5a7 | 61 | |
Tanja2211 | 9:a1890454e5a7 | 62 | //Triceps filteren |
Tanja2211 | 20:5cd52a0daab0 | 63 | arm_biquad_cascade_df1_f32(¬chT, &emg_value_f32T, &filtered_emgT, 1); |
s1340735 | 19:fdebe6892633 | 64 | arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 ); |
Tanja2211 | 9:a1890454e5a7 | 65 | filtered_emgT = fabs(filtered_emgT); |
Tanja2211 | 17:5fd768d0504f | 66 | arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 ); |
Tanja2211 | 20:5cd52a0daab0 | 67 | filtered_emgT = fabs(filtered_emgT); |
s1340735 | 21:f0357a41f43e | 68 | |
Tanja2211 | 9:a1890454e5a7 | 69 | //Triceps moving average |
Tanja2211 | 9:a1890454e5a7 | 70 | T0=filtered_emgT; |
Tanja2211 | 15:e779bfbeb8ea | 71 | 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; |
Tanja2211 | 15:e779bfbeb8ea | 72 | T29=T28; |
Tanja2211 | 15:e779bfbeb8ea | 73 | T28=T27; |
Tanja2211 | 15:e779bfbeb8ea | 74 | T27=T26; |
Tanja2211 | 15:e779bfbeb8ea | 75 | T26=T25; |
Tanja2211 | 15:e779bfbeb8ea | 76 | T25=T24; |
Tanja2211 | 15:e779bfbeb8ea | 77 | T24=T23; |
Tanja2211 | 15:e779bfbeb8ea | 78 | T23=T22; |
Tanja2211 | 15:e779bfbeb8ea | 79 | T22=T21; |
Tanja2211 | 15:e779bfbeb8ea | 80 | T21=T20; |
s1340735 | 21:f0357a41f43e | 81 | T20=T19; |
Tanja2211 | 15:e779bfbeb8ea | 82 | T19=T18; |
Tanja2211 | 15:e779bfbeb8ea | 83 | T18=T17; |
Tanja2211 | 15:e779bfbeb8ea | 84 | T17=T16; |
Tanja2211 | 15:e779bfbeb8ea | 85 | T16=T15; |
Tanja2211 | 15:e779bfbeb8ea | 86 | T15=T14; |
Tanja2211 | 15:e779bfbeb8ea | 87 | T14=T13; |
Tanja2211 | 15:e779bfbeb8ea | 88 | T13=T12; |
Tanja2211 | 15:e779bfbeb8ea | 89 | T12=T11; |
Tanja2211 | 15:e779bfbeb8ea | 90 | T11=T10; |
Tanja2211 | 15:e779bfbeb8ea | 91 | T10=T9; |
Tanja2211 | 9:a1890454e5a7 | 92 | T9=T8; |
Tanja2211 | 9:a1890454e5a7 | 93 | T8=T7; |
Tanja2211 | 9:a1890454e5a7 | 94 | T7=T6; |
Tanja2211 | 9:a1890454e5a7 | 95 | T6=T5; |
Tanja2211 | 9:a1890454e5a7 | 96 | T5=T4; |
Tanja2211 | 9:a1890454e5a7 | 97 | T4=T3; |
Tanja2211 | 9:a1890454e5a7 | 98 | T3=T2; |
Tanja2211 | 9:a1890454e5a7 | 99 | T2=T1; |
Tanja2211 | 9:a1890454e5a7 | 100 | T1=T0; |
s1340735 | 21:f0357a41f43e | 101 | |
Tanja2211 | 16:e997a6fd802a | 102 | //sturen naar scherm (Realterm) |
Tanja2211 | 16:e997a6fd802a | 103 | pc.printf("%f\r\n",MOVAVG_T); |
Tanja2211 | 16:e997a6fd802a | 104 | |
s1340735 | 10:9319e872c752 | 105 | //sturen naar HID Scope |
Tanja2211 | 9:a1890454e5a7 | 106 | scope.set(0,emg_valueT); //ruwe data |
Tanja2211 | 9:a1890454e5a7 | 107 | scope.set(1,filtered_emgT); //filtered |
Tanja2211 | 9:a1890454e5a7 | 108 | scope.send(); |
Tanja2211 | 9:a1890454e5a7 | 109 | } |
Tanja2211 | 9:a1890454e5a7 | 110 | |
s1340735 | 0:abe0bc5c43b7 | 111 | void Biceps() |
s1340735 | 0:abe0bc5c43b7 | 112 | { |
Tanja2211 | 9:a1890454e5a7 | 113 | //Biceps lezen |
s1340735 | 0:abe0bc5c43b7 | 114 | 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 | 115 | emg_value_f32B = emgB.read(); |
s1340735 | 0:abe0bc5c43b7 | 116 | |
Tanja2211 | 9:a1890454e5a7 | 117 | //Biceps filteren |
s1340735 | 19:fdebe6892633 | 118 | arm_biquad_cascade_df1_f32(¬chB, &emg_value_f32B, &filtered_emgB, 1 ); |
s1340735 | 19:fdebe6892633 | 119 | arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 ); |
s1340735 | 0:abe0bc5c43b7 | 120 | filtered_emgB = fabs(filtered_emgB); |
Tanja2211 | 17:5fd768d0504f | 121 | arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 ); |
Tanja2211 | 20:5cd52a0daab0 | 122 | filtered_emgB = fabs(filtered_emgB); |
s1340735 | 21:f0357a41f43e | 123 | |
Tanja2211 | 9:a1890454e5a7 | 124 | //Biceps moving average |
s1340735 | 0:abe0bc5c43b7 | 125 | B0=filtered_emgB; |
s1340735 | 21:f0357a41f43e | 126 | 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 | 21:f0357a41f43e | 127 | B59=B58; |
s1340735 | 21:f0357a41f43e | 128 | B58=B57; |
s1340735 | 21:f0357a41f43e | 129 | B57=B56; |
s1340735 | 21:f0357a41f43e | 130 | B56=B55; |
s1340735 | 21:f0357a41f43e | 131 | B55=B54; |
s1340735 | 21:f0357a41f43e | 132 | B54=B53; |
s1340735 | 21:f0357a41f43e | 133 | B53=B52; |
s1340735 | 21:f0357a41f43e | 134 | B52=B51; |
s1340735 | 21:f0357a41f43e | 135 | B51=B50; |
s1340735 | 21:f0357a41f43e | 136 | B50=B48; |
s1340735 | 21:f0357a41f43e | 137 | B49=B49; |
s1340735 | 21:f0357a41f43e | 138 | B48=B47; |
s1340735 | 21:f0357a41f43e | 139 | B47=B46; |
s1340735 | 21:f0357a41f43e | 140 | B46=B45; |
s1340735 | 21:f0357a41f43e | 141 | B45=B44; |
s1340735 | 21:f0357a41f43e | 142 | B44=B43; |
s1340735 | 21:f0357a41f43e | 143 | B43=B42; |
s1340735 | 21:f0357a41f43e | 144 | B42=B41; |
s1340735 | 21:f0357a41f43e | 145 | B41=B40; |
s1340735 | 21:f0357a41f43e | 146 | B40=B39; |
s1340735 | 21:f0357a41f43e | 147 | B39=B38; |
s1340735 | 21:f0357a41f43e | 148 | B38=B37; |
s1340735 | 21:f0357a41f43e | 149 | B37=B36; |
s1340735 | 21:f0357a41f43e | 150 | B36=B35; |
s1340735 | 21:f0357a41f43e | 151 | B35=B34; |
s1340735 | 21:f0357a41f43e | 152 | B34=B33; |
s1340735 | 21:f0357a41f43e | 153 | B33=B32; |
s1340735 | 21:f0357a41f43e | 154 | B32=B31; |
s1340735 | 21:f0357a41f43e | 155 | B31=B30; |
s1340735 | 21:f0357a41f43e | 156 | B30=B29; |
Tanja2211 | 15:e779bfbeb8ea | 157 | B29=B28; |
Tanja2211 | 15:e779bfbeb8ea | 158 | B28=B27; |
Tanja2211 | 15:e779bfbeb8ea | 159 | B27=B26; |
Tanja2211 | 15:e779bfbeb8ea | 160 | B26=B25; |
Tanja2211 | 15:e779bfbeb8ea | 161 | B25=B24; |
Tanja2211 | 15:e779bfbeb8ea | 162 | B24=B23; |
Tanja2211 | 15:e779bfbeb8ea | 163 | B23=B22; |
Tanja2211 | 15:e779bfbeb8ea | 164 | B22=B21; |
Tanja2211 | 15:e779bfbeb8ea | 165 | B21=B20; |
s1340735 | 21:f0357a41f43e | 166 | B20=B19; |
Tanja2211 | 15:e779bfbeb8ea | 167 | B19=B18; |
Tanja2211 | 15:e779bfbeb8ea | 168 | B18=B17; |
Tanja2211 | 15:e779bfbeb8ea | 169 | B17=B16; |
Tanja2211 | 15:e779bfbeb8ea | 170 | B16=B15; |
Tanja2211 | 15:e779bfbeb8ea | 171 | B15=B14; |
Tanja2211 | 15:e779bfbeb8ea | 172 | B14=B13; |
Tanja2211 | 15:e779bfbeb8ea | 173 | B13=B12; |
Tanja2211 | 15:e779bfbeb8ea | 174 | B12=B11; |
Tanja2211 | 15:e779bfbeb8ea | 175 | B11=B10; |
Tanja2211 | 15:e779bfbeb8ea | 176 | B10=B9; |
s1340735 | 0:abe0bc5c43b7 | 177 | B9=B8; |
s1340735 | 0:abe0bc5c43b7 | 178 | B8=B7; |
s1340735 | 0:abe0bc5c43b7 | 179 | B7=B6; |
s1340735 | 0:abe0bc5c43b7 | 180 | B6=B5; |
s1340735 | 0:abe0bc5c43b7 | 181 | B5=B4; |
s1340735 | 0:abe0bc5c43b7 | 182 | B4=B3; |
s1340735 | 0:abe0bc5c43b7 | 183 | B3=B2; |
s1340735 | 0:abe0bc5c43b7 | 184 | B2=B1; |
s1340735 | 0:abe0bc5c43b7 | 185 | B1=B0; |
s1340735 | 0:abe0bc5c43b7 | 186 | |
Tanja2211 | 16:e997a6fd802a | 187 | //sturen naar scherm |
Tanja2211 | 16:e997a6fd802a | 188 | pc.printf("%f\r\n",MOVAVG_B); |
Tanja2211 | 16:e997a6fd802a | 189 | |
s1340735 | 10:9319e872c752 | 190 | //naar HID Scope |
Tanja2211 | 6:ad6d4bd69205 | 191 | scope.set(2,emg_valueB); //ruwe data |
Tanja2211 | 6:ad6d4bd69205 | 192 | scope.set(3,filtered_emgB); //filtered |
Tanja2211 | 5:3232d78cb7b3 | 193 | scope.send(); |
s1340735 | 2:a86b09b00008 | 194 | } |
s1340735 | 0:abe0bc5c43b7 | 195 | |
s1340735 | 19:fdebe6892633 | 196 | |
s1340735 | 0:abe0bc5c43b7 | 197 | // *** MAIN *** |
s1340735 | 2:a86b09b00008 | 198 | int main() |
Tanja2211 | 9:a1890454e5a7 | 199 | { |
Tanja2211 | 9:a1890454e5a7 | 200 | pc.baud(115200); |
s1340735 | 10:9319e872c752 | 201 | |
s1340735 | 14:bebdb2ac5ec6 | 202 | //bepaling van positie met triceps 1 |
s1340735 | 14:bebdb2ac5ec6 | 203 | Ticker log_timerT1; |
s1340735 | 21:f0357a41f43e | 204 | |
s1340735 | 19:fdebe6892633 | 205 | arm_biquad_cascade_df1_init_f32(¬chT,1,notch_const,notch_states); |
Tanja2211 | 9:a1890454e5a7 | 206 | arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states); |
Tanja2211 | 9:a1890454e5a7 | 207 | arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states); |
s1340735 | 2:a86b09b00008 | 208 | |
s1340735 | 14:bebdb2ac5ec6 | 209 | log_timerT1.attach(Triceps, 0.005); |
s1340735 | 21:f0357a41f43e | 210 | wait(30); |
s1340735 | 14:bebdb2ac5ec6 | 211 | log_timerT1.detach(); |
Tanja2211 | 15:e779bfbeb8ea | 212 | |
s1340735 | 14:bebdb2ac5ec6 | 213 | MOVAVG_T=MOVAVG_Positie1; |
Tanja2211 | 15:e779bfbeb8ea | 214 | |
s1340735 | 10:9319e872c752 | 215 | // positie van batje met behulp van Triceps |
s1340735 | 14:bebdb2ac5ec6 | 216 | drempelwaardeT=0.0001; |
s1340735 | 10:9319e872c752 | 217 | |
s1340735 | 14:bebdb2ac5ec6 | 218 | if (MOVAVG_Positie1 >= drempelwaardeT) { |
s1340735 | 2:a86b09b00008 | 219 | yT1=1; |
s1340735 | 21:f0357a41f43e | 220 | |
s1340735 | 21:f0357a41f43e | 221 | //bepaling van positie met tricep 2 |
s1340735 | 21:f0357a41f43e | 222 | Ticker log_timerT2; |
s1340735 | 21:f0357a41f43e | 223 | |
s1340735 | 21:f0357a41f43e | 224 | log_timerT2.attach(Triceps, 0.005); |
s1340735 | 21:f0357a41f43e | 225 | wait(30); |
s1340735 | 21:f0357a41f43e | 226 | log_timerT2.detach(); |
s1340735 | 21:f0357a41f43e | 227 | |
s1340735 | 21:f0357a41f43e | 228 | MOVAVG_T=MOVAVG_Positie2; |
s1340735 | 21:f0357a41f43e | 229 | |
s1340735 | 14:bebdb2ac5ec6 | 230 | if (MOVAVG_Positie2 >= drempelwaardeT) { |
s1340735 | 2:a86b09b00008 | 231 | yT2=1; |
s1340735 | 2:a86b09b00008 | 232 | } else { |
s1340735 | 2:a86b09b00008 | 233 | yT2=0; |
s1340735 | 2:a86b09b00008 | 234 | } |
s1340735 | 2:a86b09b00008 | 235 | } else { |
s1340735 | 2:a86b09b00008 | 236 | yT1=0; |
s1340735 | 2:a86b09b00008 | 237 | } |
Tanja2211 | 15:e779bfbeb8ea | 238 | |
s1340735 | 10:9319e872c752 | 239 | //*** INPUT MOTOR 2 *** |
s1340735 | 10:9319e872c752 | 240 | positie=yT1+yT2; |
s1340735 | 0:abe0bc5c43b7 | 241 | |
s1340735 | 2:a86b09b00008 | 242 | //controle positie op scherm |
s1340735 | 2:a86b09b00008 | 243 | if (positie==0) { |
s1340735 | 2:a86b09b00008 | 244 | pc.printf("Motor 2 gaat naar stand 0\n"); |
s1340735 | 2:a86b09b00008 | 245 | } else { |
s1340735 | 2:a86b09b00008 | 246 | if (positie==1) { |
s1340735 | 2:a86b09b00008 | 247 | pc.printf("Motor 2 gaat naar stand 1\n"); |
s1340735 | 2:a86b09b00008 | 248 | } else { |
s1340735 | 2:a86b09b00008 | 249 | if (positie==2) { |
s1340735 | 2:a86b09b00008 | 250 | pc.printf("Motor 1 beweegt met snelheid 2\n"); |
s1340735 | 2:a86b09b00008 | 251 | } |
s1340735 | 2:a86b09b00008 | 252 | } |
s1340735 | 2:a86b09b00008 | 253 | } |
s1340735 | 10:9319e872c752 | 254 | |
s1340735 | 11:5044290660b0 | 255 | wait(5); |
s1340735 | 11:5044290660b0 | 256 | |
s1340735 | 11:5044290660b0 | 257 | Ticker log_timerB; |
s1340735 | 21:f0357a41f43e | 258 | |
s1340735 | 19:fdebe6892633 | 259 | arm_biquad_cascade_df1_init_f32(¬chB,1,notch_const,notch_states); |
s1340735 | 11:5044290660b0 | 260 | arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states); |
s1340735 | 11:5044290660b0 | 261 | arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states); |
s1340735 | 11:5044290660b0 | 262 | |
s1340735 | 11:5044290660b0 | 263 | log_timerB.attach(Biceps,0.005); |
Tanja2211 | 18:1110be4aab6e | 264 | wait(60);//log_timerB wordt 2000 keer uitgevoerd |
s1340735 | 11:5044290660b0 | 265 | log_timerB.detach(); |
s1340735 | 14:bebdb2ac5ec6 | 266 | |
s1340735 | 2:a86b09b00008 | 267 | //bepaling van snelheidsstand met biceps |
Tanja2211 | 9:a1890454e5a7 | 268 | |
s1340735 | 14:bebdb2ac5ec6 | 269 | drempelwaardeB1=0.0001; |
s1340735 | 14:bebdb2ac5ec6 | 270 | drempelwaardeB2=0.001; |
s1340735 | 14:bebdb2ac5ec6 | 271 | drempelwaardeB3=0.01; |
s1340735 | 2:a86b09b00008 | 272 | |
s1340735 | 2:a86b09b00008 | 273 | if (MOVAVG_B >= drempelwaardeB1) { |
s1340735 | 2:a86b09b00008 | 274 | yB1=1; |
s1340735 | 2:a86b09b00008 | 275 | if (MOVAVG_B >= drempelwaardeB2) { |
s1340735 | 2:a86b09b00008 | 276 | yB2=1; |
s1340735 | 2:a86b09b00008 | 277 | if (MOVAVG_B >= drempelwaardeB3) { |
s1340735 | 2:a86b09b00008 | 278 | yB3=1; |
s1340735 | 0:abe0bc5c43b7 | 279 | } else { |
s1340735 | 2:a86b09b00008 | 280 | yB3=0; |
s1340735 | 0:abe0bc5c43b7 | 281 | } |
s1340735 | 0:abe0bc5c43b7 | 282 | } else { |
s1340735 | 2:a86b09b00008 | 283 | yB2=0; |
s1340735 | 0:abe0bc5c43b7 | 284 | } |
s1340735 | 2:a86b09b00008 | 285 | } else { |
s1340735 | 2:a86b09b00008 | 286 | yB1=0; |
s1340735 | 2:a86b09b00008 | 287 | } |
s1340735 | 0:abe0bc5c43b7 | 288 | |
s1340735 | 10:9319e872c752 | 289 | //*** INPUT MOTOR 1 *** |
s1340735 | 10:9319e872c752 | 290 | snelheidsstand=yB1+yB2+yB3; |
s1340735 | 0:abe0bc5c43b7 | 291 | |
s1340735 | 2:a86b09b00008 | 292 | //controle snelheidsstand op scherm |
s1340735 | 2:a86b09b00008 | 293 | if (snelheidsstand==0) { |
s1340735 | 2:a86b09b00008 | 294 | pc.printf("Motor 1 beweegt niet\n"); |
s1340735 | 2:a86b09b00008 | 295 | } else { |
s1340735 | 1:6a8b45298e54 | 296 | if (snelheidsstand==1) { |
s1340735 | 1:6a8b45298e54 | 297 | pc.printf("Motor 1 beweegt met snelheid 1\n"); |
s1340735 | 1:6a8b45298e54 | 298 | } else { |
s1340735 | 2:a86b09b00008 | 299 | if (snelheidsstand==2) { |
s1340735 | 2:a86b09b00008 | 300 | pc.printf("Motor 1 beweegt met snelheid 2\n"); |
s1340735 | 2:a86b09b00008 | 301 | } else { |
s1340735 | 2:a86b09b00008 | 302 | if (snelheidsstand==3) { |
s1340735 | 2:a86b09b00008 | 303 | pc.printf("Motor 1 beweegt met snelheid 3\n"); |
s1340735 | 2:a86b09b00008 | 304 | } |
s1340735 | 2:a86b09b00008 | 305 | } |
s1340735 | 1:6a8b45298e54 | 306 | } |
s1340735 | 1:6a8b45298e54 | 307 | } |
s1340735 | 10:9319e872c752 | 308 | } |