Diff: main.cpp

Revision:

1:f63be2020475

Parent:

0:779fe292e912

Child:

2:0a7a3c0c08d3

--- a/main.cpp	Fri Oct 26 12:10:12 2018 +0000
+++ b/main.cpp	Fri Oct 26 14:17:28 2018 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "math.h"
 #define PI 3.14159265
 
 
@@ -6,22 +7,21 @@
 //Joe dit zijn de inputsignalen
 double theta1; double theta4; double emg1; double emg2; double emg3; 
 // Joe dit zijn de outputsignalen
-double x; double y;
+double x; double y; 
+
 //Joe dit zijn de constantes 
-double ll; double lu; double lb; double lend;
+double ll = 200.0; double lu = 170.0; double lb = 10.0; double le = 79.0; double xbase = 350.0-lb;
 //Joe dit zijn de Tickers
-Ticker emgcheck
+Ticker emgcheck;
+
 
-//forward kinematics
-double alfax = 1/2 (lb + lix + ll (Cos[theta1] - Cos[theta4]) + (
-   2 Sqrt[-(lix^2/4) + lu^2 + 
-     1/2 ll (lix (Cos[theta1] + Cos[theta4]) - ll (1 + Cos[theta1 + theta4]))] (-Sin[
-        theta1] + Sin[theta4]))/
-   Sqrt[(-(lix/ll) + Cos[theta1] + Cos[theta4])^2 + (Sin[theta1] - Sin[theta4)^2]
-double alfay = -le + ((-(lix/ll) + Cos[theta1] + Cos[theta4]) Sqrt[-(lix^2/4) + lu^2 + 
-   1/2 ll (lix (Cos[theta1] + Cos[theta4]) - ll (1 + Cos[theta1 + theta4]))])/Sqrt[(-(
-     lix/ll) + Cos[theta1] + Cos[theta4])^2 + (Sin[theta1] - Sin[theta4])^2] + 
- 1/2 ll (Sin[theta1] + Sin[theta4])
+//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(){
@@ -29,9 +29,17 @@
     //double theta1-> plus een paar counts (emg*richting)
     //double theta4-> plus een paar counts (emg*richting)
     //reken x door
+    //default = als x = xbase/2... break
     //end
     }
-void ycor(){}
+void ycor(){
+    //als emg2 == voorbij treshold,
+    //double theta1 -> plus counts (emg*richting)
+    //double theta4 -> plus counts (emg*richting)
+    //reken y door
+    //default = als y = default... break
+    //end
+    }
 void flip(){}
 
 int main()