Diff: main.cpp

Revision:

3:9ab195fa532b

diff -r 8187266f35b5 -r 9ab195fa532b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 27 15:52:40 2015 +0000
@@ -0,0 +1,42 @@
+#include "mbed.h"
+
+// >>>>> FUNCTION DEFINITION <<<<<
+// Input van deze library zijn de hoeken van de motoren. Hierdoor wordt een
+// 'real time' positie verkregen waardoor deze waarden als input gegeven kunnen worden.
+// Voor verdere informatie: zie verslag of MATLAB script.
+
+
+
+// temp NOG IN TE VOEREN!!!!!
+double theta_l = 0;
+double theta_r = 0;
+
+
+
+// >>>>> INITIALISTIONS <<<<<
+const double bar1 = 0.260; // length bar 1 [m]
+const double bar2 = 0.342; // length bar 2 [m]
+const double base_spacing = 0.10559; // spacing between the DC-motors [m]
+
+int main()
+{
+    double virt_bar_x = base_spacing + cos(theta_l)*bar1 + cos(theta_r)*bar1;
+    double virt_bar_y = sin(theta_l)*bar1 - sin(theta_r)*bar1;
+    double virt_bar = sqrt(pow(virt_bar_x,2) + pow(virt_bar_y,2));
+// virt_bar = virtuele staaf tussen de twee scharnieren van de arm
+
+    double phi = acos(0.5*virt_bar/bar2);
+    double phi_diff = atan(abs(virt_bar_y)/virt_bar_x);
+
+    double theta_l2, theta_r2;
+    if (sin(theta_l) >= sin(theta_r)) {
+        theta_l2 = phi - phi_diff;
+        theta_r2 = phi + phi_diff;
+    } else {
+        theta_l2 = phi + phi_diff;
+        theta_r2 = phi - phi_diff;
+    }
+
+    double x = -cos(theta_l)*bar1 - 0.5*base_spacing + cos(theta_l2)*bar2;
+    double y = sin(theta_l)*bar1 + sin(theta_l2)*bar2;
+}
\ No newline at end of file