emg_mk
Dependencies: HIDScope MODSERIAL mbed-dsp mbed
main.cpp@4:231bb8f5bc81, 2014-10-22 (annotated)
- Committer:
- Tanja2211
- Date:
- Wed Oct 22 09:14:21 2014 +0000
- Revision:
- 4:231bb8f5bc81
- Parent:
- 3:69ffa34e4239
- Child:
- 5:3232d78cb7b3
ingangen
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 | 3:69ffa34e4239 | 8 | HIDScope scope(4);//is dit 4 voor 2 spieren? en hoe zit het met scope.set? |
s1340735 | 0:abe0bc5c43b7 | 9 | |
Tanja2211 | 4:231bb8f5bc81 | 10 | AnalogIn emgB(PTB1);//biceps |
Tanja2211 | 4:231bb8f5bc81 | 11 | AnalogIn emgT(PTB2); // tricep |
s1340735 | 0:abe0bc5c43b7 | 12 | |
s1340735 | 0:abe0bc5c43b7 | 13 | //*** OBJECTS *** |
s1340735 | 2:a86b09b00008 | 14 | //bicep |
s1340735 | 2:a86b09b00008 | 15 | float filtered_emgB; |
s1340735 | 0:abe0bc5c43b7 | 16 | float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3 |
s1340735 | 0:abe0bc5c43b7 | 17 | int yB1, yB2, yB3; |
s1340735 | 0:abe0bc5c43b7 | 18 | float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG_B;//moving average objects |
s1340735 | 1:6a8b45298e54 | 19 | int snelheidsstand; |
s1340735 | 2:a86b09b00008 | 20 | //tricep |
s1340735 | 2:a86b09b00008 | 21 | float filtered_emgT; |
s1340735 | 2:a86b09b00008 | 22 | float drempelwaardeT1, drempelwaardeT2;//T1=positie 1, T2=positie 2 |
s1340735 | 2:a86b09b00008 | 23 | int yT1, yT2; |
s1340735 | 2:a86b09b00008 | 24 | float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, MOVAVG_T;//moving average objects |
s1340735 | 2:a86b09b00008 | 25 | int positie; |
s1340735 | 2:a86b09b00008 | 26 | |
s1340735 | 0:abe0bc5c43b7 | 27 | |
s1340735 | 0:abe0bc5c43b7 | 28 | //*** FILTERS *** |
s1340735 | 0:abe0bc5c43b7 | 29 | arm_biquad_casd_df1_inst_f32 lowpass; |
s1340735 | 0:abe0bc5c43b7 | 30 | //constants for 50Hz lowpass |
s1340735 | 0:abe0bc5c43b7 | 31 | float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator |
s1340735 | 0:abe0bc5c43b7 | 32 | float lowpass_states[4]; |
s1340735 | 0:abe0bc5c43b7 | 33 | |
s1340735 | 0:abe0bc5c43b7 | 34 | arm_biquad_casd_df1_inst_f32 highpass; |
s1340735 | 0:abe0bc5c43b7 | 35 | //constants for 10Hz highpass |
s1340735 | 0:abe0bc5c43b7 | 36 | float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2} |
s1340735 | 0:abe0bc5c43b7 | 37 | float highpass_states[4]; |
s1340735 | 0:abe0bc5c43b7 | 38 | |
s1340735 | 2:a86b09b00008 | 39 | //*** CALIBRATIE ***//dit moet nog in de main komen! en ik snap dit niet :( |
s1340735 | 2:a86b09b00008 | 40 | //void Calibratie() |
s1340735 | 2:a86b09b00008 | 41 | //{ |
s1340735 | 2:a86b09b00008 | 42 | //pc.printf("Calibratie drempelwaarde Triceps stand 1\n"); |
s1340735 | 2:a86b09b00008 | 43 | //wait(0.5); |
s1340735 | 2:a86b09b00008 | 44 | // { |
s1340735 | 2:a86b09b00008 | 45 | // int i; |
s1340735 | 2:a86b09b00008 | 46 | // int j=19; |
s1340735 | 2:a86b09b00008 | 47 | |
s1340735 | 2:a86b09b00008 | 48 | //for (i=0, i<=j; i++) { |
s1340735 | 2:a86b09b00008 | 49 | /*variable to store value in*/ |
s1340735 | 2:a86b09b00008 | 50 | // uint16_t emg_valueT1i_C; |
s1340735 | 0:abe0bc5c43b7 | 51 | |
s1340735 | 2:a86b09b00008 | 52 | // float emg_value_f32T1i_C; |
s1340735 | 2:a86b09b00008 | 53 | /*put raw emg value both in red and in emg_value*/ |
s1340735 | 2:a86b09b00008 | 54 | // emg_valueT1i_C = emgT1i_C.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) |
s1340735 | 2:a86b09b00008 | 55 | // emg_value_f32T1i_C = emgT1i_C.read(); |
s1340735 | 2:a86b09b00008 | 56 | |
s1340735 | 2:a86b09b00008 | 57 | //process emg |
s1340735 | 2:a86b09b00008 | 58 | // arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T1i_C, &filtered_emgT1i_C, 1 ); |
s1340735 | 2:a86b09b00008 | 59 | // filtered_emgT1i_C = fabs(filtered_emgT1i_C); |
s1340735 | 2:a86b09b00008 | 60 | // arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT1i_C, &filtered_emgT1i_C, 1 ); |
s1340735 | 2:a86b09b00008 | 61 | // } |
s1340735 | 2:a86b09b00008 | 62 | // } |
s1340735 | 2:a86b09b00008 | 63 | //} |
s1340735 | 2:a86b09b00008 | 64 | //****************************** |
s1340735 | 2:a86b09b00008 | 65 | //Mean Triceps stand 1 |
s1340735 | 2:a86b09b00008 | 66 | //void MeanTriceps() |
s1340735 | 2:a86b09b00008 | 67 | //{ |
s1340735 | 0:abe0bc5c43b7 | 68 | // |
s1340735 | 2:a86b09b00008 | 69 | // float MeanT1=filtered_emgT10_C*0.05+filtered_emgT11_C*0.05+filtered_emgT12_C*0.05+filtered_emgT13_C*0.05+filtered_emgT14_C*0.05+filtered_emgT15_C*0.05+filtered_emgT16_C*0.05+filtered_emgT17_C*0.05+filtered_emgT18_C*0.05+filtered_emgT19_C*0.05+filtered_emgT110_C*0.05+filtered_emgT111_C*0.05+filtered_emgT112_C*0.05+filtered_emgT113_C*0.05+filtered_emgT114_C*0.05+filtered_emgT115_C*0.05+filtered_emgT116_C*0.05+filtered_emgT117_C*0.05+filtered_emgT118_C*0.05+filtered_emgT119_C*0.05; |
s1340735 | 2:a86b09b00008 | 70 | // |
s1340735 | 2:a86b09b00008 | 71 | //} |
s1340735 | 2:a86b09b00008 | 72 | //****************************** |
s1340735 | 0:abe0bc5c43b7 | 73 | |
s1340735 | 0:abe0bc5c43b7 | 74 | //*** BICEP EMG *** |
s1340735 | 0:abe0bc5c43b7 | 75 | void Biceps() |
s1340735 | 0:abe0bc5c43b7 | 76 | { |
s1340735 | 0:abe0bc5c43b7 | 77 | uint16_t emg_valueB; |
s1340735 | 0:abe0bc5c43b7 | 78 | float emg_value_f32B; |
s1340735 | 0:abe0bc5c43b7 | 79 | |
s1340735 | 0:abe0bc5c43b7 | 80 | //lezen |
s1340735 | 0:abe0bc5c43b7 | 81 | 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 | 82 | emg_value_f32B = emgB.read(); |
s1340735 | 0:abe0bc5c43b7 | 83 | |
s1340735 | 0:abe0bc5c43b7 | 84 | //filteren |
s1340735 | 0:abe0bc5c43b7 | 85 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 ); |
s1340735 | 0:abe0bc5c43b7 | 86 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 ); |
s1340735 | 0:abe0bc5c43b7 | 87 | filtered_emgB = fabs(filtered_emgB); |
s1340735 | 0:abe0bc5c43b7 | 88 | |
s1340735 | 0:abe0bc5c43b7 | 89 | //moving average |
s1340735 | 0:abe0bc5c43b7 | 90 | B0=filtered_emgB; |
s1340735 | 0:abe0bc5c43b7 | 91 | MOVAVG_B=B0*0.1+B1*0.1+B2*0.1+B3*0.1+B4*0.1+B5*0.1+B6*0.1+B7*0.1+B8*0.1+B9*0.1; |
s1340735 | 0:abe0bc5c43b7 | 92 | B9=B8; |
s1340735 | 0:abe0bc5c43b7 | 93 | B8=B7; |
s1340735 | 0:abe0bc5c43b7 | 94 | B7=B6; |
s1340735 | 0:abe0bc5c43b7 | 95 | B6=B5; |
s1340735 | 0:abe0bc5c43b7 | 96 | B5=B4; |
s1340735 | 0:abe0bc5c43b7 | 97 | B4=B3; |
s1340735 | 0:abe0bc5c43b7 | 98 | B3=B2; |
s1340735 | 0:abe0bc5c43b7 | 99 | B2=B1; |
s1340735 | 0:abe0bc5c43b7 | 100 | B1=B0; |
s1340735 | 0:abe0bc5c43b7 | 101 | |
s1340735 | 2:a86b09b00008 | 102 | //naar scherm |
s1340735 | 2:a86b09b00008 | 103 | scope.set(0,emg_valueB); //ruwe data |
s1340735 | 2:a86b09b00008 | 104 | scope.set(1,filtered_emgB); //filtered |
s1340735 | 2:a86b09b00008 | 105 | } |
s1340735 | 0:abe0bc5c43b7 | 106 | |
s1340735 | 1:6a8b45298e54 | 107 | // *** TRICEP EMG *** |
s1340735 | 2:a86b09b00008 | 108 | void Triceps() |
s1340735 | 2:a86b09b00008 | 109 | { |
s1340735 | 2:a86b09b00008 | 110 | uint16_t emg_valueT; |
s1340735 | 2:a86b09b00008 | 111 | float emg_value_f32T; |
s1340735 | 2:a86b09b00008 | 112 | |
s1340735 | 2:a86b09b00008 | 113 | //lezen |
s1340735 | 2:a86b09b00008 | 114 | emg_valueT = emgT.read_u16(); |
s1340735 | 2:a86b09b00008 | 115 | emg_value_f32T = emgT.read(); |
s1340735 | 2:a86b09b00008 | 116 | |
s1340735 | 2:a86b09b00008 | 117 | //filteren |
s1340735 | 2:a86b09b00008 | 118 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T, &filtered_emgT, 1 ); |
s1340735 | 2:a86b09b00008 | 119 | filtered_emgT = fabs(filtered_emgT); |
s1340735 | 2:a86b09b00008 | 120 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT, &filtered_emgT, 1 ); |
s1340735 | 2:a86b09b00008 | 121 | |
s1340735 | 2:a86b09b00008 | 122 | //moving average |
s1340735 | 2:a86b09b00008 | 123 | T0=filtered_emgT; |
s1340735 | 2:a86b09b00008 | 124 | MOVAVG_T=T0*0.1+T1*0.1+T2*0.1+T3*0.1+T4*0.1+T5*0.1+T6*0.1+T7*0.1+T8*0.1+T9*0.1; |
s1340735 | 2:a86b09b00008 | 125 | T9=T8; |
s1340735 | 2:a86b09b00008 | 126 | T8=T7; |
s1340735 | 2:a86b09b00008 | 127 | T7=T6; |
s1340735 | 2:a86b09b00008 | 128 | T6=T5; |
s1340735 | 2:a86b09b00008 | 129 | T5=T4; |
s1340735 | 2:a86b09b00008 | 130 | T4=T3; |
s1340735 | 2:a86b09b00008 | 131 | T3=T2; |
s1340735 | 2:a86b09b00008 | 132 | T2=T1; |
s1340735 | 2:a86b09b00008 | 133 | T1=T0; |
s1340735 | 2:a86b09b00008 | 134 | |
s1340735 | 2:a86b09b00008 | 135 | //naar scherm |
s1340735 | 2:a86b09b00008 | 136 | scope.set(2,emg_valueT); //ruwe data |
s1340735 | 2:a86b09b00008 | 137 | scope.set(3,filtered_emgT); //filtered |
s1340735 | 2:a86b09b00008 | 138 | scope.send(); |
s1340735 | 2:a86b09b00008 | 139 | } |
s1340735 | 0:abe0bc5c43b7 | 140 | |
s1340735 | 0:abe0bc5c43b7 | 141 | // *** MAIN *** |
s1340735 | 2:a86b09b00008 | 142 | int main() |
s1340735 | 2:a86b09b00008 | 143 | { |
s1340735 | 2:a86b09b00008 | 144 | //bepaling van positie met triceps |
s1340735 | 2:a86b09b00008 | 145 | Ticker log_timerT; |
s1340735 | 2:a86b09b00008 | 146 | arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states); |
s1340735 | 2:a86b09b00008 | 147 | arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states); |
s1340735 | 2:a86b09b00008 | 148 | |
s1340735 | 2:a86b09b00008 | 149 | log_timerT.attach(Triceps, 0.005); |
s1340735 | 2:a86b09b00008 | 150 | while(1) {} |
s1340735 | 0:abe0bc5c43b7 | 151 | |
s1340735 | 2:a86b09b00008 | 152 | drempelwaardeT1=4.99; |
s1340735 | 2:a86b09b00008 | 153 | drempelwaardeT2=7; |
s1340735 | 2:a86b09b00008 | 154 | if (MOVAVG_T >= drempelwaardeT1) { |
s1340735 | 2:a86b09b00008 | 155 | yT1=1; |
s1340735 | 2:a86b09b00008 | 156 | if (MOVAVG_T >= drempelwaardeT1) { |
s1340735 | 2:a86b09b00008 | 157 | yT2=1; |
s1340735 | 2:a86b09b00008 | 158 | } else { |
s1340735 | 2:a86b09b00008 | 159 | yT2=0; |
s1340735 | 2:a86b09b00008 | 160 | } |
s1340735 | 2:a86b09b00008 | 161 | } else { |
s1340735 | 2:a86b09b00008 | 162 | yT1=0; |
s1340735 | 2:a86b09b00008 | 163 | } |
s1340735 | 0:abe0bc5c43b7 | 164 | |
s1340735 | 2:a86b09b00008 | 165 | positie=yT1+yT2;//INPUT MOTOR 2 |
s1340735 | 0:abe0bc5c43b7 | 166 | |
s1340735 | 2:a86b09b00008 | 167 | //controle positie op scherm |
s1340735 | 2:a86b09b00008 | 168 | if (positie==0) { |
s1340735 | 2:a86b09b00008 | 169 | pc.printf("Motor 2 gaat naar stand 0\n"); |
s1340735 | 2:a86b09b00008 | 170 | } else { |
s1340735 | 2:a86b09b00008 | 171 | if (positie==1) { |
s1340735 | 2:a86b09b00008 | 172 | pc.printf("Motor 2 gaat naar stand 1\n"); |
s1340735 | 2:a86b09b00008 | 173 | } else { |
s1340735 | 2:a86b09b00008 | 174 | if (positie==2) { |
s1340735 | 2:a86b09b00008 | 175 | pc.printf("Motor 1 beweegt met snelheid 2\n"); |
s1340735 | 2:a86b09b00008 | 176 | } |
s1340735 | 2:a86b09b00008 | 177 | } |
s1340735 | 2:a86b09b00008 | 178 | } |
s1340735 | 2:a86b09b00008 | 179 | |
s1340735 | 2:a86b09b00008 | 180 | |
s1340735 | 2:a86b09b00008 | 181 | //bepaling van snelheidsstand met biceps |
s1340735 | 2:a86b09b00008 | 182 | Ticker log_timerB; |
s1340735 | 2:a86b09b00008 | 183 | arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states); |
s1340735 | 2:a86b09b00008 | 184 | arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states); |
s1340735 | 2:a86b09b00008 | 185 | |
s1340735 | 2:a86b09b00008 | 186 | log_timerB.attach(Biceps, 0.005); |
s1340735 | 2:a86b09b00008 | 187 | while(1) {} |
s1340735 | 2:a86b09b00008 | 188 | |
s1340735 | 2:a86b09b00008 | 189 | drempelwaardeB1=4.99; |
s1340735 | 2:a86b09b00008 | 190 | drempelwaardeB2=6; |
s1340735 | 2:a86b09b00008 | 191 | drempelwaardeB3=10; |
s1340735 | 2:a86b09b00008 | 192 | |
s1340735 | 2:a86b09b00008 | 193 | if (MOVAVG_B >= drempelwaardeB1) { |
s1340735 | 2:a86b09b00008 | 194 | yB1=1; |
s1340735 | 2:a86b09b00008 | 195 | if (MOVAVG_B >= drempelwaardeB2) { |
s1340735 | 2:a86b09b00008 | 196 | yB2=1; |
s1340735 | 2:a86b09b00008 | 197 | if (MOVAVG_B >= drempelwaardeB3) { |
s1340735 | 2:a86b09b00008 | 198 | yB3=1; |
s1340735 | 0:abe0bc5c43b7 | 199 | } else { |
s1340735 | 2:a86b09b00008 | 200 | yB3=0; |
s1340735 | 0:abe0bc5c43b7 | 201 | } |
s1340735 | 0:abe0bc5c43b7 | 202 | } else { |
s1340735 | 2:a86b09b00008 | 203 | yB2=0; |
s1340735 | 0:abe0bc5c43b7 | 204 | } |
s1340735 | 2:a86b09b00008 | 205 | } else { |
s1340735 | 2:a86b09b00008 | 206 | yB1=0; |
s1340735 | 2:a86b09b00008 | 207 | } |
s1340735 | 0:abe0bc5c43b7 | 208 | |
s1340735 | 2:a86b09b00008 | 209 | snelheidsstand=yB1+yB2+yB3;//INPUT MOTOR 1 |
s1340735 | 0:abe0bc5c43b7 | 210 | |
s1340735 | 2:a86b09b00008 | 211 | //controle snelheidsstand op scherm |
s1340735 | 2:a86b09b00008 | 212 | if (snelheidsstand==0) { |
s1340735 | 2:a86b09b00008 | 213 | pc.printf("Motor 1 beweegt niet\n"); |
s1340735 | 2:a86b09b00008 | 214 | } else { |
s1340735 | 1:6a8b45298e54 | 215 | if (snelheidsstand==1) { |
s1340735 | 1:6a8b45298e54 | 216 | pc.printf("Motor 1 beweegt met snelheid 1\n"); |
s1340735 | 1:6a8b45298e54 | 217 | } else { |
s1340735 | 2:a86b09b00008 | 218 | if (snelheidsstand==2) { |
s1340735 | 2:a86b09b00008 | 219 | pc.printf("Motor 1 beweegt met snelheid 2\n"); |
s1340735 | 2:a86b09b00008 | 220 | } else { |
s1340735 | 2:a86b09b00008 | 221 | if (snelheidsstand==3) { |
s1340735 | 2:a86b09b00008 | 222 | pc.printf("Motor 1 beweegt met snelheid 3\n"); |
s1340735 | 2:a86b09b00008 | 223 | } |
s1340735 | 2:a86b09b00008 | 224 | } |
s1340735 | 1:6a8b45298e54 | 225 | } |
s1340735 | 1:6a8b45298e54 | 226 | } |
s1340735 | 1:6a8b45298e54 | 227 | } |