![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
emg_mk
Dependencies: HIDScope MODSERIAL mbed-dsp mbed
main.cpp@11:5044290660b0, 2014-10-23 (annotated)
- Committer:
- s1340735
- Date:
- Thu Oct 23 12:13:04 2014 +0000
- Revision:
- 11:5044290660b0
- Parent:
- 10:9319e872c752
- Child:
- 12:197b7ca8dbe2
10 s naar 60 s;
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 | |
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 |
Tanja2211 | 9:a1890454e5a7 | 15 | uint16_t emg_valueB; |
Tanja2211 | 9:a1890454e5a7 | 16 | float emg_value_f32B; |
s1340735 | 2:a86b09b00008 | 17 | float filtered_emgB; |
s1340735 | 0:abe0bc5c43b7 | 18 | float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3 |
s1340735 | 0:abe0bc5c43b7 | 19 | int yB1, yB2, yB3; |
s1340735 | 0:abe0bc5c43b7 | 20 | float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG_B;//moving average objects |
s1340735 | 1:6a8b45298e54 | 21 | int snelheidsstand; |
s1340735 | 2:a86b09b00008 | 22 | //tricep |
Tanja2211 | 9:a1890454e5a7 | 23 | uint16_t emg_valueT; |
Tanja2211 | 9:a1890454e5a7 | 24 | float emg_value_f32T; |
s1340735 | 2:a86b09b00008 | 25 | float filtered_emgT; |
s1340735 | 2:a86b09b00008 | 26 | float drempelwaardeT1, drempelwaardeT2;//T1=positie 1, T2=positie 2 |
s1340735 | 2:a86b09b00008 | 27 | int yT1, yT2; |
s1340735 | 2:a86b09b00008 | 28 | float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, MOVAVG_T;//moving average objects |
s1340735 | 2:a86b09b00008 | 29 | int positie; |
s1340735 | 2:a86b09b00008 | 30 | |
s1340735 | 0:abe0bc5c43b7 | 31 | |
s1340735 | 0:abe0bc5c43b7 | 32 | //*** FILTERS *** |
Tanja2211 | 9:a1890454e5a7 | 33 | arm_biquad_casd_df1_inst_f32 lowpassT; |
Tanja2211 | 9:a1890454e5a7 | 34 | arm_biquad_casd_df1_inst_f32 lowpassB; |
s1340735 | 0:abe0bc5c43b7 | 35 | //constants for 50Hz lowpass |
s1340735 | 0:abe0bc5c43b7 | 36 | float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator |
s1340735 | 0:abe0bc5c43b7 | 37 | float lowpass_states[4]; |
s1340735 | 0:abe0bc5c43b7 | 38 | |
Tanja2211 | 9:a1890454e5a7 | 39 | arm_biquad_casd_df1_inst_f32 highpassT; |
Tanja2211 | 9:a1890454e5a7 | 40 | arm_biquad_casd_df1_inst_f32 highpassB; |
s1340735 | 0:abe0bc5c43b7 | 41 | //constants for 10Hz highpass |
s1340735 | 0:abe0bc5c43b7 | 42 | float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2} |
s1340735 | 0:abe0bc5c43b7 | 43 | float highpass_states[4]; |
s1340735 | 0:abe0bc5c43b7 | 44 | |
s1340735 | 2:a86b09b00008 | 45 | //*** CALIBRATIE ***//dit moet nog in de main komen! en ik snap dit niet :( |
s1340735 | 2:a86b09b00008 | 46 | //void Calibratie() |
s1340735 | 2:a86b09b00008 | 47 | //{ |
s1340735 | 2:a86b09b00008 | 48 | //pc.printf("Calibratie drempelwaarde Triceps stand 1\n"); |
s1340735 | 2:a86b09b00008 | 49 | //wait(0.5); |
s1340735 | 2:a86b09b00008 | 50 | // { |
s1340735 | 2:a86b09b00008 | 51 | // int i; |
s1340735 | 2:a86b09b00008 | 52 | // int j=19; |
s1340735 | 2:a86b09b00008 | 53 | |
s1340735 | 2:a86b09b00008 | 54 | //for (i=0, i<=j; i++) { |
s1340735 | 2:a86b09b00008 | 55 | /*variable to store value in*/ |
s1340735 | 2:a86b09b00008 | 56 | // uint16_t emg_valueT1i_C; |
s1340735 | 0:abe0bc5c43b7 | 57 | |
s1340735 | 2:a86b09b00008 | 58 | // float emg_value_f32T1i_C; |
s1340735 | 2:a86b09b00008 | 59 | /*put raw emg value both in red and in emg_value*/ |
s1340735 | 2:a86b09b00008 | 60 | // 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 | 61 | // emg_value_f32T1i_C = emgT1i_C.read(); |
s1340735 | 2:a86b09b00008 | 62 | |
s1340735 | 2:a86b09b00008 | 63 | //process emg |
s1340735 | 2:a86b09b00008 | 64 | // arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T1i_C, &filtered_emgT1i_C, 1 ); |
s1340735 | 2:a86b09b00008 | 65 | // filtered_emgT1i_C = fabs(filtered_emgT1i_C); |
s1340735 | 2:a86b09b00008 | 66 | // arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT1i_C, &filtered_emgT1i_C, 1 ); |
s1340735 | 2:a86b09b00008 | 67 | // } |
s1340735 | 2:a86b09b00008 | 68 | // } |
s1340735 | 2:a86b09b00008 | 69 | //} |
s1340735 | 2:a86b09b00008 | 70 | //****************************** |
s1340735 | 2:a86b09b00008 | 71 | //Mean Triceps stand 1 |
s1340735 | 2:a86b09b00008 | 72 | //void MeanTriceps() |
s1340735 | 2:a86b09b00008 | 73 | //{ |
s1340735 | 0:abe0bc5c43b7 | 74 | // |
s1340735 | 2:a86b09b00008 | 75 | // 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 | 76 | // |
s1340735 | 2:a86b09b00008 | 77 | //} |
s1340735 | 2:a86b09b00008 | 78 | //****************************** |
s1340735 | 0:abe0bc5c43b7 | 79 | |
Tanja2211 | 9:a1890454e5a7 | 80 | |
Tanja2211 | 9:a1890454e5a7 | 81 | |
Tanja2211 | 9:a1890454e5a7 | 82 | |
Tanja2211 | 9:a1890454e5a7 | 83 | // *** TRICEPS en BICEPS EMG *** |
Tanja2211 | 9:a1890454e5a7 | 84 | void Triceps() |
Tanja2211 | 9:a1890454e5a7 | 85 | { |
Tanja2211 | 9:a1890454e5a7 | 86 | //Triceps lezen |
Tanja2211 | 9:a1890454e5a7 | 87 | emg_valueT = emgT.read_u16(); |
Tanja2211 | 9:a1890454e5a7 | 88 | emg_value_f32T = emgT.read(); |
Tanja2211 | 9:a1890454e5a7 | 89 | |
Tanja2211 | 9:a1890454e5a7 | 90 | //Triceps filteren |
Tanja2211 | 9:a1890454e5a7 | 91 | arm_biquad_cascade_df1_f32(&highpassT, &emg_value_f32T, &filtered_emgT, 1 ); |
Tanja2211 | 9:a1890454e5a7 | 92 | arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 ); |
Tanja2211 | 9:a1890454e5a7 | 93 | filtered_emgT = fabs(filtered_emgT); |
Tanja2211 | 9:a1890454e5a7 | 94 | |
s1340735 | 10:9319e872c752 | 95 | //sturen naar scherm (Realterm) |
s1340735 | 10:9319e872c752 | 96 | pc.printf("%f\r\n",filtered_emgT);//u=f filtered_emgT is een float |
Tanja2211 | 9:a1890454e5a7 | 97 | |
Tanja2211 | 9:a1890454e5a7 | 98 | //Triceps moving average |
Tanja2211 | 9:a1890454e5a7 | 99 | T0=filtered_emgT; |
Tanja2211 | 9:a1890454e5a7 | 100 | 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; |
Tanja2211 | 9:a1890454e5a7 | 101 | T9=T8; |
Tanja2211 | 9:a1890454e5a7 | 102 | T8=T7; |
Tanja2211 | 9:a1890454e5a7 | 103 | T7=T6; |
Tanja2211 | 9:a1890454e5a7 | 104 | T6=T5; |
Tanja2211 | 9:a1890454e5a7 | 105 | T5=T4; |
Tanja2211 | 9:a1890454e5a7 | 106 | T4=T3; |
Tanja2211 | 9:a1890454e5a7 | 107 | T3=T2; |
Tanja2211 | 9:a1890454e5a7 | 108 | T2=T1; |
Tanja2211 | 9:a1890454e5a7 | 109 | T1=T0; |
s1340735 | 10:9319e872c752 | 110 | |
s1340735 | 10:9319e872c752 | 111 | //sturen naar HID Scope |
Tanja2211 | 9:a1890454e5a7 | 112 | scope.set(0,emg_valueT); //ruwe data |
Tanja2211 | 9:a1890454e5a7 | 113 | scope.set(1,filtered_emgT); //filtered |
Tanja2211 | 9:a1890454e5a7 | 114 | scope.send(); |
Tanja2211 | 9:a1890454e5a7 | 115 | } |
Tanja2211 | 9:a1890454e5a7 | 116 | |
s1340735 | 0:abe0bc5c43b7 | 117 | void Biceps() |
s1340735 | 0:abe0bc5c43b7 | 118 | { |
Tanja2211 | 9:a1890454e5a7 | 119 | //Biceps lezen |
s1340735 | 0:abe0bc5c43b7 | 120 | 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 | 121 | emg_value_f32B = emgB.read(); |
s1340735 | 0:abe0bc5c43b7 | 122 | |
Tanja2211 | 9:a1890454e5a7 | 123 | //Biceps filteren |
Tanja2211 | 9:a1890454e5a7 | 124 | arm_biquad_cascade_df1_f32(&highpassB, &emg_value_f32B, &filtered_emgB, 1 ); |
Tanja2211 | 9:a1890454e5a7 | 125 | arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 ); |
s1340735 | 0:abe0bc5c43b7 | 126 | filtered_emgB = fabs(filtered_emgB); |
s1340735 | 0:abe0bc5c43b7 | 127 | |
s1340735 | 10:9319e872c752 | 128 | //sturen naar scherm |
Tanja2211 | 9:a1890454e5a7 | 129 | pc.printf("%f\r\n",filtered_emgB); |
Tanja2211 | 9:a1890454e5a7 | 130 | |
Tanja2211 | 9:a1890454e5a7 | 131 | //Biceps moving average |
s1340735 | 0:abe0bc5c43b7 | 132 | B0=filtered_emgB; |
s1340735 | 0:abe0bc5c43b7 | 133 | 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 | 134 | B9=B8; |
s1340735 | 0:abe0bc5c43b7 | 135 | B8=B7; |
s1340735 | 0:abe0bc5c43b7 | 136 | B7=B6; |
s1340735 | 0:abe0bc5c43b7 | 137 | B6=B5; |
s1340735 | 0:abe0bc5c43b7 | 138 | B5=B4; |
s1340735 | 0:abe0bc5c43b7 | 139 | B4=B3; |
s1340735 | 0:abe0bc5c43b7 | 140 | B3=B2; |
s1340735 | 0:abe0bc5c43b7 | 141 | B2=B1; |
s1340735 | 0:abe0bc5c43b7 | 142 | B1=B0; |
s1340735 | 0:abe0bc5c43b7 | 143 | |
s1340735 | 10:9319e872c752 | 144 | //naar HID Scope |
Tanja2211 | 6:ad6d4bd69205 | 145 | scope.set(2,emg_valueB); //ruwe data |
Tanja2211 | 6:ad6d4bd69205 | 146 | scope.set(3,filtered_emgB); //filtered |
Tanja2211 | 5:3232d78cb7b3 | 147 | scope.send(); |
s1340735 | 2:a86b09b00008 | 148 | } |
s1340735 | 0:abe0bc5c43b7 | 149 | |
s1340735 | 0:abe0bc5c43b7 | 150 | // *** MAIN *** |
s1340735 | 2:a86b09b00008 | 151 | int main() |
Tanja2211 | 9:a1890454e5a7 | 152 | { |
Tanja2211 | 9:a1890454e5a7 | 153 | pc.baud(115200); |
s1340735 | 10:9319e872c752 | 154 | |
s1340735 | 2:a86b09b00008 | 155 | //bepaling van positie met triceps |
s1340735 | 2:a86b09b00008 | 156 | Ticker log_timerT; |
Tanja2211 | 9:a1890454e5a7 | 157 | arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states); |
Tanja2211 | 9:a1890454e5a7 | 158 | arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states); |
s1340735 | 2:a86b09b00008 | 159 | |
s1340735 | 2:a86b09b00008 | 160 | log_timerT.attach(Triceps, 0.005); |
s1340735 | 11:5044290660b0 | 161 | wait(60); //log_timerT wordt 2000 keer uitgevoerd |
Tanja2211 | 9:a1890454e5a7 | 162 | log_timerT.detach(); |
s1340735 | 11:5044290660b0 | 163 | |
s1340735 | 10:9319e872c752 | 164 | // positie van batje met behulp van Triceps |
Tanja2211 | 7:c13686aecc75 | 165 | drempelwaardeT1=4; |
s1340735 | 2:a86b09b00008 | 166 | drempelwaardeT2=7; |
s1340735 | 10:9319e872c752 | 167 | |
s1340735 | 2:a86b09b00008 | 168 | if (MOVAVG_T >= drempelwaardeT1) { |
s1340735 | 2:a86b09b00008 | 169 | yT1=1; |
s1340735 | 2:a86b09b00008 | 170 | if (MOVAVG_T >= drempelwaardeT1) { |
s1340735 | 2:a86b09b00008 | 171 | yT2=1; |
s1340735 | 2:a86b09b00008 | 172 | } else { |
s1340735 | 2:a86b09b00008 | 173 | yT2=0; |
s1340735 | 2:a86b09b00008 | 174 | } |
s1340735 | 2:a86b09b00008 | 175 | } else { |
s1340735 | 2:a86b09b00008 | 176 | yT1=0; |
s1340735 | 2:a86b09b00008 | 177 | } |
s1340735 | 0:abe0bc5c43b7 | 178 | |
s1340735 | 10:9319e872c752 | 179 | //*** INPUT MOTOR 2 *** |
s1340735 | 10:9319e872c752 | 180 | positie=yT1+yT2; |
s1340735 | 0:abe0bc5c43b7 | 181 | |
s1340735 | 2:a86b09b00008 | 182 | //controle positie op scherm |
s1340735 | 2:a86b09b00008 | 183 | if (positie==0) { |
s1340735 | 2:a86b09b00008 | 184 | pc.printf("Motor 2 gaat naar stand 0\n"); |
s1340735 | 2:a86b09b00008 | 185 | } else { |
s1340735 | 2:a86b09b00008 | 186 | if (positie==1) { |
s1340735 | 2:a86b09b00008 | 187 | pc.printf("Motor 2 gaat naar stand 1\n"); |
s1340735 | 2:a86b09b00008 | 188 | } else { |
s1340735 | 2:a86b09b00008 | 189 | if (positie==2) { |
s1340735 | 2:a86b09b00008 | 190 | pc.printf("Motor 1 beweegt met snelheid 2\n"); |
s1340735 | 2:a86b09b00008 | 191 | } |
s1340735 | 2:a86b09b00008 | 192 | } |
s1340735 | 2:a86b09b00008 | 193 | } |
s1340735 | 10:9319e872c752 | 194 | |
s1340735 | 11:5044290660b0 | 195 | wait(5); |
s1340735 | 11:5044290660b0 | 196 | |
s1340735 | 11:5044290660b0 | 197 | Ticker log_timerB; |
s1340735 | 11:5044290660b0 | 198 | arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states); |
s1340735 | 11:5044290660b0 | 199 | arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states); |
s1340735 | 11:5044290660b0 | 200 | |
s1340735 | 11:5044290660b0 | 201 | log_timerB.attach(Biceps,0.005); |
s1340735 | 11:5044290660b0 | 202 | wait(60);//log_timerB wordt 2000 keer uitgevoerd |
s1340735 | 11:5044290660b0 | 203 | log_timerB.detach(); |
s1340735 | 11:5044290660b0 | 204 | |
s1340735 | 2:a86b09b00008 | 205 | //bepaling van snelheidsstand met biceps |
Tanja2211 | 9:a1890454e5a7 | 206 | |
Tanja2211 | 7:c13686aecc75 | 207 | drempelwaardeB1=4; |
s1340735 | 2:a86b09b00008 | 208 | drempelwaardeB2=6; |
s1340735 | 2:a86b09b00008 | 209 | drempelwaardeB3=10; |
s1340735 | 2:a86b09b00008 | 210 | |
s1340735 | 2:a86b09b00008 | 211 | if (MOVAVG_B >= drempelwaardeB1) { |
s1340735 | 2:a86b09b00008 | 212 | yB1=1; |
s1340735 | 2:a86b09b00008 | 213 | if (MOVAVG_B >= drempelwaardeB2) { |
s1340735 | 2:a86b09b00008 | 214 | yB2=1; |
s1340735 | 2:a86b09b00008 | 215 | if (MOVAVG_B >= drempelwaardeB3) { |
s1340735 | 2:a86b09b00008 | 216 | yB3=1; |
s1340735 | 0:abe0bc5c43b7 | 217 | } else { |
s1340735 | 2:a86b09b00008 | 218 | yB3=0; |
s1340735 | 0:abe0bc5c43b7 | 219 | } |
s1340735 | 0:abe0bc5c43b7 | 220 | } else { |
s1340735 | 2:a86b09b00008 | 221 | yB2=0; |
s1340735 | 0:abe0bc5c43b7 | 222 | } |
s1340735 | 2:a86b09b00008 | 223 | } else { |
s1340735 | 2:a86b09b00008 | 224 | yB1=0; |
s1340735 | 2:a86b09b00008 | 225 | } |
s1340735 | 0:abe0bc5c43b7 | 226 | |
s1340735 | 10:9319e872c752 | 227 | //*** INPUT MOTOR 1 *** |
s1340735 | 10:9319e872c752 | 228 | snelheidsstand=yB1+yB2+yB3; |
s1340735 | 0:abe0bc5c43b7 | 229 | |
s1340735 | 2:a86b09b00008 | 230 | //controle snelheidsstand op scherm |
s1340735 | 2:a86b09b00008 | 231 | if (snelheidsstand==0) { |
s1340735 | 2:a86b09b00008 | 232 | pc.printf("Motor 1 beweegt niet\n"); |
s1340735 | 2:a86b09b00008 | 233 | } else { |
s1340735 | 1:6a8b45298e54 | 234 | if (snelheidsstand==1) { |
s1340735 | 1:6a8b45298e54 | 235 | pc.printf("Motor 1 beweegt met snelheid 1\n"); |
s1340735 | 1:6a8b45298e54 | 236 | } else { |
s1340735 | 2:a86b09b00008 | 237 | if (snelheidsstand==2) { |
s1340735 | 2:a86b09b00008 | 238 | pc.printf("Motor 1 beweegt met snelheid 2\n"); |
s1340735 | 2:a86b09b00008 | 239 | } else { |
s1340735 | 2:a86b09b00008 | 240 | if (snelheidsstand==3) { |
s1340735 | 2:a86b09b00008 | 241 | pc.printf("Motor 1 beweegt met snelheid 3\n"); |
s1340735 | 2:a86b09b00008 | 242 | } |
s1340735 | 2:a86b09b00008 | 243 | } |
s1340735 | 1:6a8b45298e54 | 244 | } |
s1340735 | 1:6a8b45298e54 | 245 | } |
Tanja2211 | 9:a1890454e5a7 | 246 | |
s1340735 | 10:9319e872c752 | 247 | } |