Jan-Willem Bisschop
EMG filter. Waarden nog niet perfekt.
Revision 1:70dc835f3a37, committed 2016-10-28
- Comitter:
- janwillembisschop
- Date:
- Fri Oct 28 08:55:20 2016 +0000
- Parent:
- 0:2956e3501f8a
- Commit message:
- EMG filter met goede waarden
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter2.cpp Fri Oct 28 08:55:20 2016 +0000
@@ -0,0 +1,14 @@
+#include "Filter2.h"
+
+double Filter2(double u, double &w1, double &w2, const double a1, const double a2, const double b0, const double b1, const double b2, const double gain)
+{
+
+ double w = u - a1*w1 - a2*w2;
+ double y = gain * (b0*w + b1*w1 + b2*w2);
+
+ w2 = w1;
+ w1 = w;
+
+ return y;
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Filter2.h Fri Oct 28 08:55:20 2016 +0000 @@ -0,0 +1,3 @@ +#include "mbed.h" + +double Filter2(double u, double &w1, double &w2, const double a1, const double a2, const double b0, const double b1, const double b2, const double gain); \ No newline at end of file
--- a/FilterDesign.cpp Fri Oct 21 12:11:52 2016 +0000
+++ b/FilterDesign.cpp Fri Oct 28 08:55:20 2016 +0000
@@ -1,29 +1,47 @@
#include "FilterDesign.h"
#include "Filter.h"
+ // Notch 50 Hz
double v1_50 = 0, v2_50 = 0;
- const double a1_50 = -1.52337295428, a2_50 = 0.93195385841;
- const double b0_50 = 1.00000000000, b1_50 = -1.61854514929, b2_50 = 1.00000000000;
- const double gain_50 = 1.00000000000;
+ const double a1_50 = -1.53713228931, a2_50 = 0.90250000000;
+ const double b0_50 = 1.00000000000, b1_50 = -1.61803398875, b2_50 = 1.00000000000;
+ const double gain_50 = 0.95500000000;
+
+ // Notch 100 Hz
+ double v1_100 = 0, v2_100 = 0;
+ const double a1_100 = -0.58713228931, a2_100 = 0.90250000000;
+ const double b0_100 = 1.00000000000, b1_100 = -0.61803398875, b2_100 = 1.00000000000;
+ const double gain_100 = 0.95200000000;
+ // Notch 200 Hz
+ double v1_200 = 0, v2_200 = 0;
+ const double a1_200 = 1.53713228931, a2_200 = 0.90250000000;
+ const double b0_200 = 1.00000000000, b1_200 = 1.61803398875, b2_200 = 1.00000000000;
+ const double gain_200 = 0.95500000000;
+
+ // High pass 50 Hz
+ double v1_HP = 0, v2_HP = 0;
+ const double a1_HP = -1.47413760026, a2_HP = 0.60062500000;
+ const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
+ const double gain_HP = 0.77000000000;
+
+ // Low pass 0.7 Hz
double v1_LP = 0, v2_LP = 0;
- const double a1_LP = -1.93503824887, a2_LP = 0.93708289663;
+ const double a1_LP = -1.80000000000, a2_LP = 0.81000000000;
const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
- const double gain_LP = 0.000511;
-
- double v1_HP = 0, v2_HP = 0;
- const double a1_HP = -0.76475499450, a2_HP = 0.27692273367;
- const double b0_HP = 1.00000000000, b1_HP = -2.0, b2_HP = 1.00000000000;
- const double gain_HP = 0.510419;
+ const double gain_LP = 0.0025100000000;
double FilterDesign(double u)
{
double y_50 = Filter(u,v1_50,v2_50,a1_50,a2_50,b0_50,b1_50,b2_50,gain_50);
- double y_HP = Filter(y_50,v1_HP,v2_HP,a1_HP,a2_HP,b0_HP,b1_HP,b2_HP,gain_HP);
+ double y_100 = Filter(y_50,v1_100,v2_100,a1_100,a2_100,b0_100,b1_100,b2_100,gain_100);
+ double y_200 = Filter(y_100,v1_200,v2_200,a1_200,a2_200,b0_200,b1_200,b2_200,gain_200);
+ double y_HP = Filter(y_200,v1_HP,v2_HP,a1_HP,a2_HP,b0_HP,b1_HP,b2_HP,gain_HP);
double y_abs = fabs(y_HP);
double y_LP = Filter(y_abs,v1_LP,v2_LP,a1_LP,a2_LP,b0_LP,b1_LP,b2_LP,gain_LP);
- double y = 10 + y_LP;
+
+ double y = y_LP;
return y;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/FilterDesign2.cpp Fri Oct 28 08:55:20 2016 +0000
@@ -0,0 +1,48 @@
+#include "FilterDesign2.h"
+#include "Filter2.h"
+
+ // Notch 50 Hz
+ double w1_50 = 0, w2_50 = 0;
+ const double a1_50 = -1.53713228931, a2_50 = 0.90250000000;
+ const double b0_50 = 1.00000000000, b1_50 = -1.61803398875, b2_50 = 1.00000000000;
+ const double gain_50 = 0.95500000000;
+
+ // Notch 100 Hz
+ double w1_100 = 0, w2_100 = 0;
+ const double a1_100 = -0.58713228931, a2_100 = 0.90250000000;
+ const double b0_100 = 1.00000000000, b1_100 = -0.61803398875, b2_100 = 1.00000000000;
+ const double gain_100 = 0.95200000000;
+
+ // Notch 200 Hz
+ double w1_200 = 0, w2_200 = 0;
+ const double a1_200 = 1.53713228931, a2_200 = 0.90250000000;
+ const double b0_200 = 1.00000000000, b1_200 = 1.61803398875, b2_200 = 1.00000000000;
+ const double gain_200 = 0.95500000000;
+
+ // High pass 50 Hz
+ double w1_HP = 0, w2_HP = 0;
+ const double a1_HP = -1.47413760026, a2_HP = 0.60062500000;
+ const double b0_HP = 1.00000000000, b1_HP = -2.00000000000, b2_HP = 1.00000000000;
+ const double gain_HP = 0.77000000000;
+
+ // Low pass 0.7 Hz
+ double w1_LP = 0, w2_LP = 0;
+ const double a1_LP = -1.80000000000, a2_LP = 0.81000000000;
+ const double b0_LP = 1.00000000000, b1_LP = 2.00000000000, b2_LP = 1.00000000000;
+ const double gain_LP = 0.0025100000000;
+
+double FilterDesign2(double u)
+{
+
+ double y_50 = Filter2(u,w1_50,w2_50,a1_50,a2_50,b0_50,b1_50,b2_50,gain_50);
+ double y_100 = Filter2(y_50,w1_100,w2_100,a1_100,a2_100,b0_100,b1_100,b2_100,gain_100);
+ double y_200 = Filter2(y_100,w1_200,w2_200,a1_200,a2_200,b0_200,b1_200,b2_200,gain_200);
+ double y_HP = Filter2(y_200,w1_HP,w2_HP,a1_HP,a2_HP,b0_HP,b1_HP,b2_HP,gain_HP);
+ double y_abs = fabs(y_HP);
+ double y_LP = Filter2(y_abs,w1_LP,w2_LP,a1_LP,a2_LP,b0_LP,b1_LP,b2_LP,gain_LP);
+
+ double y = y_LP;
+
+ return y;
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FilterDesign2.h Fri Oct 28 08:55:20 2016 +0000 @@ -0,0 +1,3 @@ +#include "mbed.h" + +double FilterDesign2(double u); \ No newline at end of file
--- a/main.cpp Fri Oct 21 12:11:52 2016 +0000
+++ b/main.cpp Fri Oct 28 08:55:20 2016 +0000
@@ -2,28 +2,109 @@
#include "HIDScope.h"
#include "Filter.h"
#include "FilterDesign.h"
+#include "Filter2.h"
+#include "FilterDesign2.h"
-AnalogIn emg(A0);
+AnalogIn emg1(A0);
+AnalogIn emg2(A1);
Ticker sample_timer;
-HIDScope scope(2); // 2 scopes
+HIDScope scope(4); // 2 scopes
+
+
+// Motor aanroepen
+DigitalOut motor_1(D7); //richting (1 of 0)
+PwmOut pwm_motor_1(D6); //snelheid (tussen 0 en 1?)
+DigitalOut motor_2(D4); //richting (1 of 0)
+PwmOut pwm_motor_2(D5); //snelheid (tussen 0 en 1?)
+InterruptIn stop(SW3); //stoppen
+InterruptIn turn_1(D9); //wisselen van kant (button 1)
+InterruptIn turn_2(D10); //wisselen van kant (button 2)
+Serial pc(USBTX, USBRX); //USB ports aanroepen
+Ticker faster_slower; //Ticker voor functie om sneller en langzamer te gaan
+
+DigitalOut led1(D2);
+DigitalOut led2(D3);
+
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_blue(LED_BLUE);
+
+volatile double y1;
+volatile double y2;
+volatile double u1;
+volatile double u2;
void sample()
{
- double u = emg.read(); // ongefitlerd
- double y = FilterDesign(u); // gefilterd
+ double u1 = emg1.read(); // ongefitlerd
+ double y1 = FilterDesign(u1); // gefilterd
+ double u2 = emg2.read(); // ongefitlerd
+ double y2 = FilterDesign2(u2); // gefilterd
- scope.set(0,u); // ongefilterd naar scope 0
- scope.set(1,y); // gefilterd naar scope 1
+ scope.set(0,u1); // ongefilterd emg1 naar scope 0
+ scope.set(1,y1); // gefilterd emg1 naar scope 1
+ scope.set(2,u2); // ongefilterd emg2 naar scope 2
+ scope.set(3,y2); // gefilterd emg2 naar scope 3
scope.send();
+
+ /*
+ if (y1 > 0.05)
+ {
+ motor_1 = 1;
+ pwm_motor_1 = 0.6;
+ }
+ else
+ {
+ pwm_motor_1 = 0;
+ }
+
+ if (y2 > 0.05)
+ {
+ motor_2 = 1;
+ pwm_motor_2 = 0.6;
+ }
+ else
+ {
+ pwm_motor_2 = 0;
+ }
+ */
+
+ if (y1 >= 0.06)
+ {
+ led_red.write(0); // aan
+ led_green.write(1); // uit
+ led_blue.write(1); // uit
+ }
+ if (y2 >= 0.09)
+ {
+ led_red.write(1); // uit
+ led_green.write(0); // aan
+ led_blue.write(1); // uit
+ }
+ if (y1 >= 0.06 & y2 >= 0.09)
+ {
+ led_red.write(1); // uit
+ led_green.write(1); // uit
+ led_blue.write(0); // aan
+ }
+ else if (y1 < 0.06 & y2 < 0.09)
+ {
+ led_red.write(1); // uit
+ led_green.write(1); // uit
+ led_blue.write(1); // uit
+ }
+
}
+
+
int main()
{
-
- sample_timer.attach(&sample, 0.002); // 500 Hz
+ pc.baud(115200);
+ sample_timer.attach(&sample, 0.01); // 500 Hz
while(1) {}
}
\ No newline at end of file