Timo de Vries / Mbed 2 deprecated EMG_filtering

Filtering works for emg

Dependencies:   HIDScope MODSERIAL mbed

Fork of EMG by Tom Tom

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?

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 );
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 }