Diff: main.cpp

Revision:

0:1acd9371fe29

Child:

1:8bc06db8386e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 19 08:59:09 2015 +0000
@@ -0,0 +1,55 @@
+#include "mbed.h"
+
+// >>>>> FUNCTION DEFINITION <<<<<
+// Input van deze library is de huidige (x,y)-positie van de pod. Hierdoor wordt een 
+// reeks hoeken verkregen waardoor de robot een schietbeweging in y-richting uitvoerd. 
+// Voor verdere informatie: zie verslag of MATLAB script.
+
+
+
+
+
+
+// temp NOG IN TE VOEREN!!!!!
+double x = 1;
+double y = 1;
+const double y_punch = 0.473;
+
+
+
+
+
+
+
+// >>>>> 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]
+const double pi = 3.1415926535897;
+
+int main()
+{
+double virt_bar_left = sqrt(pow(x+0.5*base_spacing,2) + pow(y,2));
+double virt_bar_right = sqrt(pow(x-0.5*base_spacing,2) + pow(y,2));
+double virt_bar_ref = sqrt(pow(bar2,2)- pow(bar1,2));
+
+// >>>>> LEFT ARM <<<<<
+double d2_square = pow(bar1,2) - pow((pow(virt_bar_left,2) + pow(bar1,2) - pow(bar2,2))/(-2*virt_bar_left),2);
+double d2 = sqrt(d2_square);
+double phi;
+if (virt_bar_left < virt_bar_ref)
+    phi = pi-asin(d2/bar1);
+else
+    phi = asin(d2/bar1);
+    
+double theta_l = pi - phi - acos((x + 0.5*base_spacing)/virt_bar_left);
+
+// >>>>> RIGHT ARM <<<<<
+d2_square = pow(bar1,2) - pow((pow(virt_bar_right,2) + pow(bar1,2) - pow(bar2,2))/(-2*virt_bar_right),2);
+d2 = sqrt(d2_square);
+if (virt_bar_right < virt_bar_ref)
+    phi = pi-asin(d2/bar1);
+else
+    phi = asin(d2/bar1);
+double theta_r = phi + acos((x - 0.5*base_spacing)/virt_bar_right);
+}
\ No newline at end of file