Filtering works for emg
Dependencies: HIDScope MODSERIAL mbed
Fork of EMG by
main.cpp@22:ad85b8acf8b5, 2016-10-24 (annotated)
- Committer:
- Frostworks
- Date:
- Mon Oct 24 14:45:10 2016 +0000
- Revision:
- 22:ad85b8acf8b5
- Parent:
- 21:2b55d53e11f6
Left and right are filterd
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:32bb76391d89 | 1 | #include "mbed.h" |
vsluiter | 11:ce72ec658a95 | 2 | #include "HIDScope.h" |
Frostworks | 21:2b55d53e11f6 | 3 | #include "MODSERIAL.h" |
vsluiter | 0:32bb76391d89 | 4 | |
vsluiter | 4:8b298dfada81 | 5 | //Define objects |
tomlankhorst | 19:2bf824669684 | 6 | AnalogIn emg0( A0 ); |
tomlankhorst | 19:2bf824669684 | 7 | AnalogIn emg1( A1 ); |
Frostworks | 22:ad85b8acf8b5 | 8 | DigitalIn buttonCalibrate(SW2); |
Frostworks | 21:2b55d53e11f6 | 9 | MODSERIAL pc(USBTX, USBRX); |
Frostworks | 21:2b55d53e11f6 | 10 | |
Frostworks | 21:2b55d53e11f6 | 11 | volatile float x; |
Frostworks | 21:2b55d53e11f6 | 12 | volatile float x_prev =0; |
Frostworks | 21:2b55d53e11f6 | 13 | volatile float b; // filtered 'output' of ReadAnalogInAndFilter |
Frostworks | 21:2b55d53e11f6 | 14 | |
Frostworks | 22:ad85b8acf8b5 | 15 | bool calibrate = false; |
Frostworks | 22:ad85b8acf8b5 | 16 | double threshold_Left = 0; |
Frostworks | 22:ad85b8acf8b5 | 17 | double threshold_Right= 0; |
tomlankhorst | 14:f83354387756 | 18 | Ticker sample_timer; |
tomlankhorst | 19:2bf824669684 | 19 | HIDScope scope( 2 ); |
tomlankhorst | 18:21d8e7a81cf5 | 20 | DigitalOut led(LED1); |
Frostworks | 21:2b55d53e11f6 | 21 | const double a1 = -1.6475; |
Frostworks | 21:2b55d53e11f6 | 22 | const double a2 = 0.7009; |
Frostworks | 21:2b55d53e11f6 | 23 | const double b0 = 0.8371; |
Frostworks | 21:2b55d53e11f6 | 24 | const double b1 = -1.6742; |
Frostworks | 21:2b55d53e11f6 | 25 | const double b2 = 0.8371; |
Frostworks | 21:2b55d53e11f6 | 26 | const double c1 = -1.9645; |
Frostworks | 21:2b55d53e11f6 | 27 | const double c2 = 0.9651; |
Frostworks | 21:2b55d53e11f6 | 28 | const double d0 = 0.0001551; |
Frostworks | 21:2b55d53e11f6 | 29 | const double d1 = 0.0003103; |
Frostworks | 21:2b55d53e11f6 | 30 | const double d2 = 0.0001551; |
Frostworks | 21:2b55d53e11f6 | 31 | double v1_high = 0; |
Frostworks | 21:2b55d53e11f6 | 32 | double v2_high = 0; |
Frostworks | 21:2b55d53e11f6 | 33 | double v1_low = 0; |
Frostworks | 21:2b55d53e11f6 | 34 | double v2_low = 0; |
Frostworks | 22:ad85b8acf8b5 | 35 | double highpassFilterLeft = 0; |
Frostworks | 22:ad85b8acf8b5 | 36 | double lowpassFilterLeft = 0; |
Frostworks | 22:ad85b8acf8b5 | 37 | double highpassFilterRight = 0; |
Frostworks | 22:ad85b8acf8b5 | 38 | double lowpassFilterRight = 0; |
vsluiter | 2:e314bb3b2d99 | 39 | |
Frostworks | 21:2b55d53e11f6 | 40 | double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0, |
Frostworks | 21:2b55d53e11f6 | 41 | const double b1, const double b2) |
Frostworks | 21:2b55d53e11f6 | 42 | { |
Frostworks | 21:2b55d53e11f6 | 43 | double v = u - a1*v1 - a2*v2; |
Frostworks | 21:2b55d53e11f6 | 44 | double y = b0*v + b1*v1 + b2*v2; |
Frostworks | 21:2b55d53e11f6 | 45 | v2 = v1; |
Frostworks | 21:2b55d53e11f6 | 46 | v1 = v; |
Frostworks | 21:2b55d53e11f6 | 47 | return y; |
Frostworks | 21:2b55d53e11f6 | 48 | } |
Frostworks | 22:ad85b8acf8b5 | 49 | /*double biquad2(double u, double&v1, double&v2, const double c1, const double c2, const double d0, |
Frostworks | 21:2b55d53e11f6 | 50 | const double d1, const double d2) |
Frostworks | 21:2b55d53e11f6 | 51 | { |
Frostworks | 21:2b55d53e11f6 | 52 | double v = u - c1*v1 - c2*v2; |
Frostworks | 21:2b55d53e11f6 | 53 | double y = d0*v + d1*v1 + d2*v2; |
Frostworks | 21:2b55d53e11f6 | 54 | v2 = v1; |
Frostworks | 21:2b55d53e11f6 | 55 | v1 = v; |
Frostworks | 21:2b55d53e11f6 | 56 | return y; |
Frostworks | 21:2b55d53e11f6 | 57 | } |
Frostworks | 22:ad85b8acf8b5 | 58 | */ |
tomlankhorst | 14:f83354387756 | 59 | /** Sample function |
tomlankhorst | 14:f83354387756 | 60 | * this function samples the emg and sends it to HIDScope |
tomlankhorst | 14:f83354387756 | 61 | **/ |
Frostworks | 21:2b55d53e11f6 | 62 | |
Frostworks | 22:ad85b8acf8b5 | 63 | void filterSampleLeft() |
Frostworks | 21:2b55d53e11f6 | 64 | { |
Frostworks | 22:ad85b8acf8b5 | 65 | highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2)); |
Frostworks | 22:ad85b8acf8b5 | 66 | lowpassFilterLeft = biquad1(highpassFilterLeft, v1_low, v2_low, c1, c2, d0, d1, d2); |
Frostworks | 22:ad85b8acf8b5 | 67 | scope.set(0, lowpassFilterLeft ); |
Frostworks | 21:2b55d53e11f6 | 68 | scope.send(); |
Frostworks | 22:ad85b8acf8b5 | 69 | //pc.printf("%f \n \r ", lowpassFilter); |
Frostworks | 21:2b55d53e11f6 | 70 | } |
Frostworks | 22:ad85b8acf8b5 | 71 | void filterSampleRight() |
Frostworks | 22:ad85b8acf8b5 | 72 | { |
Frostworks | 22:ad85b8acf8b5 | 73 | highpassFilterRight = fabs(biquad1(emg1.read(), v1_high, v2_high, a1, a2, b0, b1, b2)); |
Frostworks | 22:ad85b8acf8b5 | 74 | lowpassFilterRight = biquad1(highpassFilterRight, v1_low, v2_low, c1, c2, d0, d1, d2); |
Frostworks | 22:ad85b8acf8b5 | 75 | scope.set(1, lowpassFilterRight ); |
Frostworks | 22:ad85b8acf8b5 | 76 | scope.send(); |
Frostworks | 22:ad85b8acf8b5 | 77 | //pc.printf("%f \n \r ", lowpassFilter); |
Frostworks | 22:ad85b8acf8b5 | 78 | } |
tomlankhorst | 14:f83354387756 | 79 | void sample() |
vsluiter | 2:e314bb3b2d99 | 80 | { |
tomlankhorst | 19:2bf824669684 | 81 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
tomlankhorst | 19:2bf824669684 | 82 | scope.set(0, emg0.read() ); |
tomlankhorst | 19:2bf824669684 | 83 | scope.set(1, emg1.read() ); |
Frostworks | 21:2b55d53e11f6 | 84 | /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) |
tomlankhorst | 19:2bf824669684 | 85 | * Ensure that enough channels are available (HIDScope scope( 2 )) |
tomlankhorst | 20:97059009a491 | 86 | * Finally, send all channels to the PC at once */ |
Frostworks | 21:2b55d53e11f6 | 87 | |
Frostworks | 21:2b55d53e11f6 | 88 | x = emg0; // Capture data scope.set(0, x); // store data in first element of scope memory |
Frostworks | 21:2b55d53e11f6 | 89 | b = (x_prev + x)/2.0; // averaging filter |
Frostworks | 21:2b55d53e11f6 | 90 | x_prev = x; // Prepare for next round |
Frostworks | 21:2b55d53e11f6 | 91 | |
vsluiter | 11:ce72ec658a95 | 92 | scope.send(); |
tomlankhorst | 18:21d8e7a81cf5 | 93 | /* To indicate that the function is working, the LED is toggled */ |
tomlankhorst | 18:21d8e7a81cf5 | 94 | led = !led; |
Frostworks | 21:2b55d53e11f6 | 95 | pc.printf("%f, %f \n \r ", x, b); |
vsluiter | 2:e314bb3b2d99 | 96 | } |
vsluiter | 0:32bb76391d89 | 97 | |
vsluiter | 0:32bb76391d89 | 98 | int main() |
Frostworks | 21:2b55d53e11f6 | 99 | { |
tomlankhorst | 14:f83354387756 | 100 | /**Attach the 'sample' function to the timer 'sample_timer'. |
tomlankhorst | 19:2bf824669684 | 101 | * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz |
vsluiter | 4:8b298dfada81 | 102 | */ |
Frostworks | 22:ad85b8acf8b5 | 103 | //sample_timer.attach(&sample, 0.001953125); |
Frostworks | 22:ad85b8acf8b5 | 104 | sample_timer.attach(&filterSampleLeft, 0.001953125); //512 Hz |
Frostworks | 22:ad85b8acf8b5 | 105 | sample_timer.attach(&filterSampleRight, 0.001953125); |
Frostworks | 21:2b55d53e11f6 | 106 | pc.baud(115200); |
Frostworks | 22:ad85b8acf8b5 | 107 | pc.printf("please push the button to calibrate \n \r"); |
Frostworks | 22:ad85b8acf8b5 | 108 | while (1) { |
Frostworks | 22:ad85b8acf8b5 | 109 | if (buttonCalibrate == 0) { |
Frostworks | 22:ad85b8acf8b5 | 110 | calibrate = true; |
Frostworks | 22:ad85b8acf8b5 | 111 | threshold_Left = lowpassFilterLeft*0.7; |
Frostworks | 22:ad85b8acf8b5 | 112 | threshold_Right = lowpassFilterRight*0.7; |
Frostworks | 21:2b55d53e11f6 | 113 | } |
Frostworks | 22:ad85b8acf8b5 | 114 | if (calibrate == true) { |
Frostworks | 22:ad85b8acf8b5 | 115 | pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right); |
Frostworks | 22:ad85b8acf8b5 | 116 | /*empty loop, sample() is executed periodically*/ |
Frostworks | 22:ad85b8acf8b5 | 117 | } |
Frostworks | 22:ad85b8acf8b5 | 118 | } |
vsluiter | 0:32bb76391d89 | 119 | } |