Diff: main.cpp

Revision:

5:d78ed3a3e66a

Parent:

4:49dfbfcd3577

Child:

6:59744dfe8ea7

diff -r 49dfbfcd3577 -r d78ed3a3e66a main.cpp
--- a/main.cpp	Mon Oct 29 15:25:33 2018 +0000
+++ b/main.cpp	Mon Oct 29 15:46:20 2018 +0000
@@ -20,10 +20,10 @@
 
 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
 //First define the position equation of x
-double xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
-double xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
-double xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
-double xend = (xendsum + xendsqrt1/xendsqrt2)/2;
+double xendsum;
+double xendsqrt1;
+double xendsqrt2;
+double xend;
 //Now define the pos. eq. of y
 double yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
 double yendsqrt1 = (-xbase/ll + cos(theta1)+cos(theta4))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1)+cos(theta4))- ll*(1+cos(theta1+theta4)))); 
@@ -66,8 +66,12 @@
     //default = theta1 = theta4 = pi/2 
     emgcheck.attach(xcor, 0.1);
     while(true){
-       pc.printf("%f %f \n", xend,yend);
-       wait(1);
+              
+       if (button1 == 0){
+        pc.printf("\n\r %f %f \n\r", xend,yend);
+        pc.printf(" %f\n\r", theta1);
+        wait(1.5);
+        }
         }