Timo de Vries / Mbed 2 deprecated EMG_filtering

Filtering works for emg

Dependencies:   HIDScope MODSERIAL mbed

Fork of EMG by Tom Tom

main.cpp@21:2b55d53e11f6, 2016-10-24 (annotated)

Committer:
Frostworks
Date:
Mon Oct 24 13:33:09 2016 +0000
Revision:
21:2b55d53e11f6
Parent:
20:97059009a491
Child:
22:ad85b8acf8b5
it works with the filtered signal

Who changed what in which revision?

UserRevisionLine numberNew 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 );
tomlankhorst 19:2bf824669684 8
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
tomlankhorst 14:f83354387756 15 Ticker sample_timer;
tomlankhorst 19:2bf824669684 16 HIDScope scope( 2 );
tomlankhorst 18:21d8e7a81cf5 17 DigitalOut led(LED1);
Frostworks 21:2b55d53e11f6 18 const double a1 = -1.6475;
Frostworks 21:2b55d53e11f6 19 const double a2 = 0.7009;
Frostworks 21:2b55d53e11f6 20 const double b0 = 0.8371;
Frostworks 21:2b55d53e11f6 21 const double b1 = -1.6742;
Frostworks 21:2b55d53e11f6 22 const double b2 = 0.8371;
Frostworks 21:2b55d53e11f6 23 const double c1 = -1.9645;
Frostworks 21:2b55d53e11f6 24 const double c2 = 0.9651;
Frostworks 21:2b55d53e11f6 25 const double d0 = 0.0001551;
Frostworks 21:2b55d53e11f6 26 const double d1 = 0.0003103;
Frostworks 21:2b55d53e11f6 27 const double d2 = 0.0001551;
Frostworks 21:2b55d53e11f6 28 double v1_high = 0;
Frostworks 21:2b55d53e11f6 29 double v2_high = 0;
Frostworks 21:2b55d53e11f6 30 double v1_low = 0;
Frostworks 21:2b55d53e11f6 31 double v2_low = 0;
Frostworks 21:2b55d53e11f6 32 double highpassFilter = 0;
Frostworks 21:2b55d53e11f6 33 double lowpassFilter = 0;
vsluiter 2:e314bb3b2d99 34
Frostworks 21:2b55d53e11f6 35 double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
Frostworks 21:2b55d53e11f6 36 const double b1, const double b2)
Frostworks 21:2b55d53e11f6 37 {
Frostworks 21:2b55d53e11f6 38 double v = u - a1*v1 - a2*v2;
Frostworks 21:2b55d53e11f6 39 double y = b0*v + b1*v1 + b2*v2;
Frostworks 21:2b55d53e11f6 40 v2 = v1;
Frostworks 21:2b55d53e11f6 41 v1 = v;
Frostworks 21:2b55d53e11f6 42 return y;
Frostworks 21:2b55d53e11f6 43 }
Frostworks 21:2b55d53e11f6 44 double biquad2(double u, double&v1, double&v2, const double c1, const double c2, const double d0,
Frostworks 21:2b55d53e11f6 45 const double d1, const double d2)
Frostworks 21:2b55d53e11f6 46 {
Frostworks 21:2b55d53e11f6 47 double v = u - c1*v1 - c2*v2;
Frostworks 21:2b55d53e11f6 48 double y = d0*v + d1*v1 + d2*v2;
Frostworks 21:2b55d53e11f6 49 v2 = v1;
Frostworks 21:2b55d53e11f6 50 v1 = v;
Frostworks 21:2b55d53e11f6 51 return y;
Frostworks 21:2b55d53e11f6 52 }
Frostworks 21:2b55d53e11f6 53 double biquad3(double u, double&v1, double&v2, const double c1, const double c2, const double d0,
Frostworks 21:2b55d53e11f6 54 const double d1, const double d2)
Frostworks 21:2b55d53e11f6 55 {
Frostworks 21:2b55d53e11f6 56 double v = u - c1*v1 - c2*v2;
Frostworks 21:2b55d53e11f6 57 double y = d0*v + d1*v1 + d2*v2;
Frostworks 21:2b55d53e11f6 58 v2 = v1;
Frostworks 21:2b55d53e11f6 59 v1 = v;
Frostworks 21:2b55d53e11f6 60 return y;
Frostworks 21:2b55d53e11f6 61 }
tomlankhorst 14:f83354387756 62 /** Sample function
tomlankhorst 14:f83354387756 63 * this function samples the emg and sends it to HIDScope
tomlankhorst 14:f83354387756 64 **/
Frostworks 21:2b55d53e11f6 65
Frostworks 21:2b55d53e11f6 66 void filterSample()
Frostworks 21:2b55d53e11f6 67 {
Frostworks 21:2b55d53e11f6 68 highpassFilter = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
Frostworks 21:2b55d53e11f6 69 lowpassFilter = biquad2(highpassFilter, v1_low, v2_low, c1, c2, d0, d1, d2);
Frostworks 21:2b55d53e11f6 70 scope.set(0, lowpassFilter );
Frostworks 21:2b55d53e11f6 71 scope.send();
Frostworks 21:2b55d53e11f6 72 pc.printf("%f \n \r ", lowpassFilter);
Frostworks 21:2b55d53e11f6 73 }
Frostworks 21:2b55d53e11f6 74
tomlankhorst 14:f83354387756 75 void sample()
vsluiter 2:e314bb3b2d99 76 {
tomlankhorst 19:2bf824669684 77 /* 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 78 scope.set(0, emg0.read() );
tomlankhorst 19:2bf824669684 79 scope.set(1, emg1.read() );
Frostworks 21:2b55d53e11f6 80 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
tomlankhorst 19:2bf824669684 81 * Ensure that enough channels are available (HIDScope scope( 2 ))
tomlankhorst 20:97059009a491 82 * Finally, send all channels to the PC at once */
Frostworks 21:2b55d53e11f6 83
Frostworks 21:2b55d53e11f6 84 x = emg0; // Capture data scope.set(0, x); // store data in first element of scope memory
Frostworks 21:2b55d53e11f6 85 b = (x_prev + x)/2.0; // averaging filter
Frostworks 21:2b55d53e11f6 86 x_prev = x; // Prepare for next round
Frostworks 21:2b55d53e11f6 87
vsluiter 11:ce72ec658a95 88 scope.send();
tomlankhorst 18:21d8e7a81cf5 89 /* To indicate that the function is working, the LED is toggled */
tomlankhorst 18:21d8e7a81cf5 90 led = !led;
Frostworks 21:2b55d53e11f6 91 pc.printf("%f, %f \n \r ", x, b);
vsluiter 2:e314bb3b2d99 92 }
vsluiter 0:32bb76391d89 93
vsluiter 0:32bb76391d89 94 int main()
Frostworks 21:2b55d53e11f6 95 {
tomlankhorst 14:f83354387756 96 /**Attach the 'sample' function to the timer 'sample_timer'.
tomlankhorst 19:2bf824669684 97 * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
vsluiter 4:8b298dfada81 98 */
Frostworks 21:2b55d53e11f6 99
Frostworks 21:2b55d53e11f6 100
Frostworks 21:2b55d53e11f6 101 //sample_timer.attach(&sample, 0.002);
Frostworks 21:2b55d53e11f6 102 sample_timer.attach(&filterSample, 0.002);
Frostworks 21:2b55d53e11f6 103 pc.baud(115200);
Frostworks 21:2b55d53e11f6 104 pc.printf("hoi\n");
tomlankhorst 14:f83354387756 105 /*empty loop, sample() is executed periodically*/
Frostworks 21:2b55d53e11f6 106 while(1) {
Frostworks 21:2b55d53e11f6 107
Frostworks 21:2b55d53e11f6 108 }
vsluiter 0:32bb76391d89 109 }