Diff: main.cpp

Revision:

4:49dfbfcd3577

Parent:

3:de8d3ca44a3e

Child:

5:d78ed3a3e66a

--- a/main.cpp	Mon Oct 29 15:01:52 2018 +0000
+++ b/main.cpp	Mon Oct 29 15:25:33 2018 +0000
@@ -37,14 +37,17 @@
 
 //Joe, hieronder staan de functies die door de tickers aangeroepen worden
 void xcor(){
-    if (button1 == 0){
-                            //als emg1==voorbij treshold, 
-    theta1 = PI*(theta1/PI + 0.1);        //double theta1-> plus een paar counts (emg*richting)
-    theta4 = PI*0.5;          //double theta4-> plus een paar counts (emg*richting)
+    
+    if (button1 == 0){                          //als emg1==voorbij treshold, 
+        theta1 = PI*(theta1/PI + 0.1);          //double theta1-> plus een paar counts (emg*richting)
+                                                //double theta4-> plus een paar counts (emg*richting)
+    //default = als x = xbase/2... break, okee dit moet hier niet
+    }
+    else {theta1 = theta1;}
+    xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
+    xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
+    xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
     xend = (xendsum + xendsqrt1/xendsqrt2)/2;
-    //default = als x = xbase/2... break
-    //end
-    }
 }
 void ycor(){
     //als emg2 == voorbij treshold,
@@ -61,12 +64,10 @@
     
     pc.baud(115200);
     //default = theta1 = theta4 = pi/2 
-    emgcheck.attach(xcor, 0.001);
-    emgcheck.attach(ycor, 0.001);
-    emgcheck.attach(flip, 0.001);
+    emgcheck.attach(xcor, 0.1);
     while(true){
-        pc.printf("%f %f \n", xend,yend);
-        wait(1);
+       pc.printf("%f %f \n", xend,yend);
+       wait(1);
         }