Revision 5:07d15b93b361, committed 2015-10-16

Comitter:

marijnstudent

Date:

Fri Oct 16 11:23:33 2015 +0000

Parent:

4:833948d12d78

Commit message:

losse functie getangles

Changed in this revision

--- a/main.cpp	Fri Oct 16 10:52:36 2015 +0000
+++ b/main.cpp	Fri Oct 16 11:23:33 2015 +0000
@@ -12,119 +12,29 @@
 
 
 
-// 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 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;
-
-// 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 x_input;
+double y_input;
+    void getangles(double &theta_one,double &theta_two,double x_input,double y_input)
 {
-    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()
-{
-// 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();
-
-    // xx en yy zijn de gewenste x en y waardes, de hoeken theta 1 en 2 worden berekend
-    double xx = 7*50*y5;
-    double yy = 7*50*y5t;
+    double xx = x_input;
+    double yy = y_input;
     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(yy,xx)+beta;
     theta_two = pi + alfa;
-    
-    // testen van de functie
-    scope.set(0,xx);
-    scope.set(1,yy);
-    double xt = 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();
+
 }
 
 int main()
 {
 
-    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.
-
+getangles(theta_one,theta_two, x_input, y_input);
     
 }