Diff: angleandposition.cpp

Revision:

2:b99fef8df97e

Parent:

1:6cace9fdb088

Child:

3:067c75c62882

--- a/angleandposition.cpp	Mon Oct 19 13:25:40 2015 +0000
+++ b/angleandposition.cpp	Mon Oct 19 15:05:21 2015 +0000
@@ -2,9 +2,9 @@
 #include "mbed.h"
 #include "math.h"
 
-const double bar1 = 260; // length bar 1 [mm]
-const double bar2 = 342; // length bar 2 [mm]
-const double base_spacing = 105.59; // spacing between the DC-motors [mm]
+const double bar1 = 0.260; // length bar 1 [mm]
+const double bar2 = 0.342; // length bar 2 [mm]
+const double base_spacing = 0.10559; // spacing between the DC-motors [mm]
 const double pi = 3.1415926535897;
 
 double virt_bar_ref = sqrt(pow(bar2,2)- pow(bar1,2));
@@ -25,17 +25,17 @@
     
     double phi;
     if (virt_bar_right <= virt_bar_ref) {
-        double   phi = pi-asin(d2/bar1);
+          phi = pi-asin(d2/bar1);
     } else {
-        double phi = asin(d2/bar1);
+        phi = asin(d2/bar1);
     }
-    double theta_r = phi + acos((x_position - 0.5*base_spacing)/virt_bar_right);
+    double theta_r = -phi + acos((x_position - 0.5*base_spacing)/virt_bar_right);
     return theta_r;
 }
 
 float angleandposition::positiontoangle2(float x_position, float y_position)//left arm
 {
-    double virt_bar_left = sqrt(pow(x_position + 0.5*base_spacing ,2) + pow(y_position,2) );
+    double virt_bar_left = sqrt(pow((x_position + 0.5*base_spacing) ,2) + pow(y_position,2) );
     
     double d2_square = pow(bar1,2) - pow( ( pow(virt_bar_left,2) + pow(bar1,2) - pow(bar2,2) ) / (-2*virt_bar_left), 2);
     
@@ -44,9 +44,9 @@
     double phi;
     
     if (virt_bar_left <= virt_bar_ref) {
-        double  phi = pi-asin(d2/bar1);
+         phi = pi-asin(d2/bar1);
     } else {
-        double phi = asin(d2/bar1);
+     phi = asin(d2/bar1);
     }
 
     double theta_l = pi - phi - acos((x_position + 0.5*base_spacing)/virt_bar_left);