Revision 3:c1d11616f252, committed 2015-10-16

Comitter:

marijnstudent

Date:

Fri Oct 16 10:50:13 2015 +0000

Parent:

2:e2af01393e16

Child:

4:833948d12d78

Commit message:

testfunctie om positie te testen

Changed in this revision

--- a/main.cpp	Fri Oct 16 08:56:32 2015 +0000
+++ b/main.cpp	Fri Oct 16 10:50:13 2015 +0000
@@ -1,32 +1,117 @@
 #include "mbed.h"
 #include "math.h"
 #include "HIDScope.h"
+#include "complex.h"
 
 HIDScope scope(4);
-AnalogIn    potright(A0);
-AnalogIn    potleft(A1);
+// Analoge input signalen defineren
+AnalogIn    EMG_in(A0);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A0 uit te lezen
+AnalogIn    EMG_int(A2);                // deze leest A2 uit
+// ticker met de naam tick om je control functie uit te lezen.
+Ticker      tick;
+
+
+
 // x & y zijn inputs L is lengte van de arm r is de vector naar de end effector
 
 const double L = 36;
 const double pi = 3.1415926535897;
-double theta1;
-double theta2;
+double theta_one;
+double theta_two;
+
+// tweede orde notch filter 50 Hz
+// biquad 1 coefficienten
+const double numnotch50biq1_1 = 1;
+const double numnotch50biq1_2 = -1.61816178466632;
+const double numnotch50biq1_3 = 1.00000006127058;
+const double dennotch50biq1_2 = -1.59325742941798;
+const double dennotch50biq1_3 = 0.982171881701431;
+// biquad 2 coefficienten
+const double numnotch50biq2_1 = 1;
+const double numnotch50biq2_2 = -1.61816171933244;
+const double numnotch50biq2_3 = 0.999999938729428;
+const double dennotch50biq2_2 = -1.61431180968071;
+const double dennotch50biq2_3 = 0.982599066293075;
+
+// highpass filter 20 Hz coefficienten
+const double numhigh20_1 = 0.837089190566345;
+const double numhigh20_2 = -1.67417838113269;
+const double numhigh20_3 = 0.837089190566345;
+const double denhigh20_2 = -1.64745998107698;
+const double denhigh20_3 = 0.700896781188403;
+
+// lowpass 5 Hz coefficienten
+const double numlow5_1 =0.000944691843840162;
+const double numlow5_2 =0.00188938368768032;
+const double numlow5_3 =0.000944691843840162;
+const double denlow5_2 =-1.91119706742607;
+const double denlow5_3 =0.914975834801434;
+
+// storage variables voor 8 filters aanmaken. Voor een signaal zijn dit 4 biquads, namelijk notch(2), highpass(1) & lowpass(1)
+double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0;
+double f1_v1t = 0, f1_v2t = 0, f2_v1t = 0, f2_v2t = 0, f3_v1t = 0, f3_v2t = 0,f4_v1t = 0, f4_v2t = 0;
 
-// functies die de hoeken berekend
-void getangles(double &theta_one,double &theta_two) // xy inputs
+// biquadfilters die bij het filteren van signaal 1 horen
+double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
+{
+    double v = u- a1*v1-a2*v2;
+    double y = b0*v+b1*v1+b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
+// biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee)
+double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t)
+{
+    double vt = ut- a1t*v1t-a2t*v2t;
+    double yt = b0t*vt+b1t*v1t+b2t*v2t;
+    v2t = v1t;
+    v1t = vt;
+    return yt;
+}
+
+
+
+// myController vraagt de analoge waardes op (A0 en A2, d.m.v. AnalogIn[t].read())
+// deze waardes worden gefilterd en naar HIDScope gestuurd
+void myController()
 {
-    double x = 60+ 20*potleft.read();
-    double y = 40+40*potright.read();
-    double r = sqrt(pow(L,2)+pow(L,2)); // vector naar end effector
-    double alfa = acos((2*pow(L,2)-pow(r,2))/(4*L)); // alfa is de hoek tussen upper en lower arm
+// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
+    double u1 = EMG_in.read();
+    double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+    double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+    double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
+    double y4 = abs(y3);
+    double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
+// versturen van het input signaal u1 en het gefilterde signaal y5 naar HIDScope channel 0 en 1
+    //scope.set(0,u1);
+    //scope.set(1,y5);
+// filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
+    double u1t = EMG_int.read();
+    double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+    double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+    double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
+    double y4t = abs(y3t);
+    double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
+    // versturen van input signaal2; u1t en het gefilterde signaal; y5t naar HIDScope channel 2 en 3
+    //scope.set(2,u1t);
+    //scope.set(3,y5t);
+    // verzenden van de verstuurde signalen
+    //scope.send();
+
+    double xx = 7*50*y5;
+    double yy = 7*50*y5t;
+    double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
+    double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
     double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
     // hoeken berekenen
-    theta_one = atan2(y,x)+beta;
+    theta_one = atan2(yy,xx)+beta;
     theta_two = pi + alfa;
-    scope.set(0,x);
-    scope.set(1,y);
+    scope.set(0,xx);
+    scope.set(1,yy);
     double xt = L*cos(theta_one)+L*cos(theta_one+theta_two);
-    double yt = L*cos(theta_one)+L*cos(theta_one+theta_two);
+    double yt = L*sin(theta_one)+L*sin(theta_one+theta_two);
     scope.set(2,xt);
     scope.set(3,yt);
     scope.send();
@@ -34,7 +119,9 @@
 
 int main()
 {
-    while(true) {
-        getangles(theta1,theta2);
-    }
+
+    tick.attach(&myController,0.01f);
+    // roep elke 0.01 seconde de functie myController aan, dus elke 0.01 seconde wordt er gesamplet en dit sample gefilterd.
+
+    
 }