Diff: main.cpp

Revision:

1:8bc06db8386e

Parent:

0:1acd9371fe29

--- a/main.cpp	Mon Oct 19 08:59:09 2015 +0000
+++ b/main.cpp	Tue Oct 20 12:57:58 2015 +0000
@@ -1,23 +1,15 @@
 #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. 
+// 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 x = 1;
-double y = 1;
-const double y_punch = 0.473;
-
-
-
-
+double theta_l = 0;
+double theta_r = 0;
 
 
 
@@ -25,31 +17,26 @@
 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));
+    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);
 
-// >>>>> 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);
+    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;
+    }
 
-// >>>>> 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);
+    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