Diff: main.cpp

Revision:

2:0a7a3c0c08d3

Parent:

1:f63be2020475

Child:

3:de8d3ca44a3e

--- a/main.cpp	Fri Oct 26 14:17:28 2018 +0000
+++ b/main.cpp	Mon Oct 29 14:02:47 2018 +0000
@@ -1,8 +1,13 @@
 #include "mbed.h"
-#include "math.h"
+#include <math.h>
+#include <cmath>
+#include "MODSERIAL.h"
 #define PI 3.14159265
 
-
+MODSERIAL pc(USBTX, USBRX);
+DigitalIn button1(SW3);
+DigitalIn button2(SW2);
+DigitalOut LED(LED1);
 
 //Joe dit zijn de inputsignalen
 double theta1; double theta4; double emg1; double emg2; double emg3; 
@@ -10,25 +15,32 @@
 double x; double y; 
 
 //Joe dit zijn de constantes 
-double ll = 200.0; double lu = 170.0; double lb = 10.0; double le = 79.0; double xbase = 350.0-lb;
+double ll = 200.0; double lu = 170.0; double lb = 10.0; double le = 79.0; double xbase = 340;
 //Joe dit zijn de Tickers
+
+//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;
+//Now define the pos. eq. of y
+double yendsum = le;
+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)))); 
+double yendsqrt2 = ll/5*(sin(theta1)+sin(theta4))+ sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
+float yend = (xendsum + xendsqrt1/xendsqrt2);
+
+
+//Hier definieren we de functies
 Ticker emgcheck;
 
 
-//forward kinematics, Check mathematica!
-double alfax = 1.0/2.0 *(lb + xbase + ll*(cos(theta1) - cos(theta4)) + (2.0*pow(-(pow(xbase, 2.0))/4 + lu^2 + 1/2*ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))),0.5)*
-(-sin(theta1) + sin(theta4)))/sqrt((-(xbase/ll) + cos(theta1) + cos(theta4))^2 + (sin(theta1) - sin(theta4)^2)
-double alfay = -le + ((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-(xbase^2/4) + lu^2 + 
-   1/2 ll (xbase (cos(theta1) + cos(theta4)) - ll (1 + cos(theta1 + theta4)))))/ sqrt((-(
-     xbase/ll) + cos(theta1) + cos(theta4))^2 + (sin(theta1) - sin(theta4))^2) + 
- 1/2 ll (sin(theta1) + sin(theta4));
-
 //Joe, hieronder staan de functies die door de tickers aangeroepen worden
 void xcor(){
-    //als emg1==voorbij treshold, 
-    //double theta1-> plus een paar counts (emg*richting)
-    //double theta4-> plus een paar counts (emg*richting)
-    //reken x door
+                            //als emg1==voorbij treshold, 
+    theta1 = PI*0.33;        //double theta1-> plus een paar counts (emg*richting)
+    theta4 = PI*0.5;          //double theta4-> plus een paar counts (emg*richting)
+    xend = (xendsum + xendsqrt1/xendsqrt2)/2;
     //default = als x = xbase/2... break
     //end
     }
@@ -44,6 +56,9 @@
 
 int main()
 {
+    
+    pc.baud(115200);
+    pc.printf("%f\n", xend);
     //default = theta1 = theta4 = pi/2 
     emgcheck.attach(xcor, 0.001);
     emgcheck.attach(ycor, 0.001);
@@ -51,4 +66,5 @@
     
     
     
+    
 }
\ No newline at end of file