Diff: angleandposition.cpp

Revision:

3:067c75c62882

Parent:

2:b99fef8df97e

Child:

4:9492c1c361b9

--- a/angleandposition.cpp	Mon Oct 19 15:05:21 2015 +0000
+++ b/angleandposition.cpp	Tue Oct 27 16:13:26 2015 +0000
@@ -22,10 +22,10 @@
     double d2_square = pow(bar1,2) - pow( ( pow(virt_bar_right,2) + pow(bar1,2) - pow(bar2,2) ) / (-2*virt_bar_right) ,2);
 
     double d2 = sqrt(d2_square);
-    
+
     double phi;
     if (virt_bar_right <= virt_bar_ref) {
-          phi = pi-asin(d2/bar1);
+        phi = pi-asin(d2/bar1);
     } else {
         phi = asin(d2/bar1);
     }
@@ -36,20 +36,63 @@
 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 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);
+        phi = pi-asin(d2/bar1);
     } else {
-     phi = asin(d2/bar1);
+        phi = asin(d2/bar1);
     }
 
     double theta_l = pi - phi - acos((x_position + 0.5*base_spacing)/virt_bar_left);
 
     return theta_l;
+}
+
+float angleandposition::angletoposition(float theta_r,float theta_l)
+{
+
+    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));
+
+
+    double phi = acos(0.5*virt_bar/bar2);
+    double phi_diff = atan(abs(virt_bar_y)/virt_bar_x);
+
+    double theta_l2;
+    if (sin(theta_l) >= sin(theta_r)) {
+        theta_l2 = phi - phi_diff;
+    } else {
+        theta_l2 = phi + phi_diff;
+    }
+
+    double x = -cos(theta_l)*bar1 - 0.5*base_spacing + cos(theta_l2)*bar2;
+
+    return x;
+}
+
+float angleandposition::angletoposition_y (float theta_r, float theta_l)
+{
+    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));
+
+    double phi = acos(0.5*virt_bar/bar2);
+    double phi_diff = atan(abs(virt_bar_y)/virt_bar_x);
+
+    double theta_l2;
+    if (sin(theta_l) >= sin(theta_r)) {
+        theta_l2 = phi - phi_diff;
+    } else {
+        theta_l2 = phi + phi_diff;
+    }
+    double y = sin(theta_l)*bar1 + sin(theta_l2)*bar2;
+
+    return y;
 }
\ No newline at end of file