Diff: main.cpp

Revision:

0:779fe292e912

Child:

1:f63be2020475

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 26 12:10:12 2018 +0000
@@ -0,0 +1,46 @@
+#include "mbed.h"
+#define PI 3.14159265
+
+
+
+//Joe dit zijn de inputsignalen
+double theta1; double theta4; double emg1; double emg2; double emg3; 
+// Joe dit zijn de outputsignalen
+double x; double y;
+//Joe dit zijn de constantes 
+double ll; double lu; double lb; double lend;
+//Joe dit zijn de Tickers
+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])
+
+//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
+    //end
+    }
+void ycor(){}
+void flip(){}
+
+int main()
+{
+    //default = theta1 = theta4 = pi/2 
+    emgcheck.attach(xcor, 0.001);
+    emgcheck.attach(ycor, 0.001);
+    emgcheck.attach(flip, 0.001);
+    
+    
+    
+}
\ No newline at end of file