test
Dependencies: HIDScope MODSERIAL mbed-dsp mbed
Fork of emg_filter2 by
EMGfilter.cpp@27:24e73fd36859, 2014-10-17 (annotated)
- Committer:
- s1340735
- Date:
- Fri Oct 17 08:38:15 2014 +0000
- Revision:
- 27:24e73fd36859
- Parent:
- 26:b93c82fb6e1d
- Child:
- 29:f54123765a47
void loopers
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
s1340735 | 22:dc630dbb1dcd | 1 | #include "mbed.h" |
s1340735 | 22:dc630dbb1dcd | 2 | #include "HIDScope.h" |
s1340735 | 23:1c51af8386c9 | 3 | #include "MODSERIAL.h" |
s1340735 | 22:dc630dbb1dcd | 4 | #include "arm_math.h" |
s1340735 | 22:dc630dbb1dcd | 5 | |
s1340735 | 22:dc630dbb1dcd | 6 | //Define objects |
s1340735 | 22:dc630dbb1dcd | 7 | AnalogIn emgB(PTB0); //Analog input bicep |
s1340735 | 22:dc630dbb1dcd | 8 | AnalogIn emgT(PTB1); //Analog input tricep |
s1340735 | 22:dc630dbb1dcd | 9 | |
s1340735 | 23:1c51af8386c9 | 10 | float filtered_emgB; |
s1340735 | 27:24e73fd36859 | 11 | float filtered_emgT; |
s1340735 | 23:1c51af8386c9 | 12 | |
s1340735 | 23:1c51af8386c9 | 13 | MODSERIAL pc(USBTX,USBRX); |
s1340735 | 23:1c51af8386c9 | 14 | |
s1340735 | 26:b93c82fb6e1d | 15 | HIDScope scope(4);//uitgang scherm |
s1340735 | 22:dc630dbb1dcd | 16 | |
s1340735 | 22:dc630dbb1dcd | 17 | arm_biquad_casd_df1_inst_f32 lowpass; |
s1340735 | 22:dc630dbb1dcd | 18 | //constants for 50Hz lowpass |
s1340735 | 26:b93c82fb6e1d | 19 | float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator |
s1340735 | 22:dc630dbb1dcd | 20 | //state values |
s1340735 | 26:b93c82fb6e1d | 21 | float lowpass_states[4]; |
s1340735 | 22:dc630dbb1dcd | 22 | |
s1340735 | 22:dc630dbb1dcd | 23 | arm_biquad_casd_df1_inst_f32 highpass; |
s1340735 | 22:dc630dbb1dcd | 24 | //constants for 10Hz highpass |
s1340735 | 26:b93c82fb6e1d | 25 | float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2} |
s1340735 | 22:dc630dbb1dcd | 26 | //state values |
s1340735 | 26:b93c82fb6e1d | 27 | float highpass_states[4]; |
s1340735 | 22:dc630dbb1dcd | 28 | |
s1340735 | 22:dc630dbb1dcd | 29 | |
s1340735 | 22:dc630dbb1dcd | 30 | /** Looper function |
s1340735 | 22:dc630dbb1dcd | 31 | * functions used for Ticker and Timeout should be of type void <name>(void) |
s1340735 | 22:dc630dbb1dcd | 32 | * i.e. no input arguments, no output arguments. |
s1340735 | 22:dc630dbb1dcd | 33 | * if you want to change a variable that you use in other places (for example in main) |
s1340735 | 22:dc630dbb1dcd | 34 | * you will have to make that variable global in order to be able to reach it both from |
s1340735 | 22:dc630dbb1dcd | 35 | * the function called at interrupt time, and in the main function. |
s1340735 | 22:dc630dbb1dcd | 36 | * To make a variable global, define it under the includes. |
s1340735 | 22:dc630dbb1dcd | 37 | * variables that are changed in the interrupt routine (written to) should be made |
s1340735 | 22:dc630dbb1dcd | 38 | * 'volatile' to let the compiler know that those values may change outside the current context. |
s1340735 | 22:dc630dbb1dcd | 39 | * i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value" |
s1340735 | 22:dc630dbb1dcd | 40 | * in the example below, the variable is not re-used in the main function, and is thus declared |
s1340735 | 22:dc630dbb1dcd | 41 | * local in the looper function only. |
s1340735 | 22:dc630dbb1dcd | 42 | **/ |
s1340735 | 22:dc630dbb1dcd | 43 | |
s1340735 | 22:dc630dbb1dcd | 44 | //BICEP EMG LEZEN |
s1340735 | 22:dc630dbb1dcd | 45 | void looperB() |
s1340735 | 22:dc630dbb1dcd | 46 | { |
s1340735 | 23:1c51af8386c9 | 47 | /*variable to store value in*/ |
s1340735 | 22:dc630dbb1dcd | 48 | uint16_t emg_valueB; |
s1340735 | 23:1c51af8386c9 | 49 | |
s1340735 | 22:dc630dbb1dcd | 50 | float emg_value_f32B; |
s1340735 | 22:dc630dbb1dcd | 51 | /*put raw emg value both in red and in emg_value*/ |
s1340735 | 22:dc630dbb1dcd | 52 | emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) |
s1340735 | 22:dc630dbb1dcd | 53 | emg_value_f32B = emgB.read(); |
s1340735 | 22:dc630dbb1dcd | 54 | |
s1340735 | 22:dc630dbb1dcd | 55 | //process emg |
s1340735 | 22:dc630dbb1dcd | 56 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 ); |
s1340735 | 22:dc630dbb1dcd | 57 | filtered_emgB = fabs(filtered_emgB); |
s1340735 | 22:dc630dbb1dcd | 58 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 ); |
s1340735 | 23:1c51af8386c9 | 59 | |
s1340735 | 22:dc630dbb1dcd | 60 | /*send value to PC. */ |
s1340735 | 22:dc630dbb1dcd | 61 | scope.set(0,emg_valueB); //uint value |
s1340735 | 22:dc630dbb1dcd | 62 | scope.set(1,filtered_emgB); //processed float |
s1340735 | 22:dc630dbb1dcd | 63 | scope.send(); |
s1340735 | 22:dc630dbb1dcd | 64 | |
s1340735 | 22:dc630dbb1dcd | 65 | } |
s1340735 | 22:dc630dbb1dcd | 66 | |
s1340735 | 22:dc630dbb1dcd | 67 | void looperT() |
s1340735 | 22:dc630dbb1dcd | 68 | { |
s1340735 | 23:1c51af8386c9 | 69 | /*variable to store value in*/ |
s1340735 | 22:dc630dbb1dcd | 70 | uint16_t emg_valueT; |
s1340735 | 27:24e73fd36859 | 71 | |
s1340735 | 22:dc630dbb1dcd | 72 | float emg_value_f32T; |
s1340735 | 22:dc630dbb1dcd | 73 | /*put raw emg value both in red and in emg_value*/ |
s1340735 | 22:dc630dbb1dcd | 74 | emg_valueT = emgT.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) |
s1340735 | 22:dc630dbb1dcd | 75 | emg_value_f32T = emgT.read(); |
s1340735 | 22:dc630dbb1dcd | 76 | |
s1340735 | 22:dc630dbb1dcd | 77 | //process emg |
s1340735 | 22:dc630dbb1dcd | 78 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T, &filtered_emgT, 1 ); |
s1340735 | 22:dc630dbb1dcd | 79 | filtered_emgT = fabs(filtered_emgT); |
s1340735 | 22:dc630dbb1dcd | 80 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT, &filtered_emgT, 1 ); |
s1340735 | 23:1c51af8386c9 | 81 | |
s1340735 | 22:dc630dbb1dcd | 82 | /*send value to PC. */ |
s1340735 | 27:24e73fd36859 | 83 | scope.set(2,emg_valueT); //uint value |
s1340735 | 27:24e73fd36859 | 84 | scope.set(3,filtered_emgT); //processed float |
s1340735 | 22:dc630dbb1dcd | 85 | scope.send(); |
s1340735 | 22:dc630dbb1dcd | 86 | |
s1340735 | 22:dc630dbb1dcd | 87 | } |
s1340735 | 22:dc630dbb1dcd | 88 | |
s1340735 | 22:dc630dbb1dcd | 89 | int main() |
s1340735 | 22:dc630dbb1dcd | 90 | { |
s1340735 | 22:dc630dbb1dcd | 91 | Ticker log_timer; |
s1340735 | 23:1c51af8386c9 | 92 | //set up filters. Use external array for constants |
s1340735 | 22:dc630dbb1dcd | 93 | arm_biquad_cascade_df1_init_f32(&lowpass,1 , lowpass_const, lowpass_states); |
s1340735 | 22:dc630dbb1dcd | 94 | arm_biquad_cascade_df1_init_f32(&highpass,1 ,highpass_const, highpass_states); |
s1340735 | 22:dc630dbb1dcd | 95 | |
s1340735 | 22:dc630dbb1dcd | 96 | /**Here you attach the 'void looper(void)' function to the Ticker object |
s1340735 | 22:dc630dbb1dcd | 97 | * The looper() function will be called every 0.01 seconds. |
s1340735 | 22:dc630dbb1dcd | 98 | * Please mind that the parentheses after looper are omitted when using attach. |
s1340735 | 22:dc630dbb1dcd | 99 | */ |
s1340735 | 22:dc630dbb1dcd | 100 | log_timer.attach(looperB, 0.005);//?? |
s1340735 | 22:dc630dbb1dcd | 101 | log_timer.attach(looperT, 0.005);//?? |
s1340735 | 23:1c51af8386c9 | 102 | while(1) { //Loop |
s1340735 | 23:1c51af8386c9 | 103 | /*Empty!*/ |
s1340735 | 23:1c51af8386c9 | 104 | /*Everything is handled by the interrupt routine now!*/ |
s1340735 | 23:1c51af8386c9 | 105 | } |
s1340735 | 23:1c51af8386c9 | 106 | } |
s1340735 | 23:1c51af8386c9 | 107 | |
s1340735 | 22:dc630dbb1dcd | 108 | //filtered_emgB |
s1340735 | 22:dc630dbb1dcd | 109 | //filtered_emgT |
s1340735 | 22:dc630dbb1dcd | 110 | |
s1340735 | 23:1c51af8386c9 | 111 | void Antwoord() |
s1340735 | 23:1c51af8386c9 | 112 | { |
s1340735 | 23:1c51af8386c9 | 113 | float drempelwaarde=4.99; |
s1340735 | 25:cfd6db9b4b5d | 114 | int y; |
s1340735 | 25:cfd6db9b4b5d | 115 | |
s1340735 | 25:cfd6db9b4b5d | 116 | if (filtered_emgB > drempelwaarde) |
s1340735 | 25:cfd6db9b4b5d | 117 | { |
s1340735 | 25:cfd6db9b4b5d | 118 | y=1; |
s1340735 | 23:1c51af8386c9 | 119 | } else { |
s1340735 | 25:cfd6db9b4b5d | 120 | y=0; |
s1340735 | 23:1c51af8386c9 | 121 | } |
s1340735 | 23:1c51af8386c9 | 122 | |
s1340735 | 25:cfd6db9b4b5d | 123 | if (y==1) { |
s1340735 | 25:cfd6db9b4b5d | 124 | pc.printf("Motor 1 beweegt\n"); |
s1340735 | 23:1c51af8386c9 | 125 | } else { |
s1340735 | 25:cfd6db9b4b5d | 126 | pc.printf("Motor 1 beweegt niet\n"); |
s1340735 | 22:dc630dbb1dcd | 127 | } |
s1340735 | 24:553707c8ebf8 | 128 | } |
s1340735 | 25:cfd6db9b4b5d | 129 | //drempelwaarde..... |