Revision 5:2dc21bf3f3a0, committed 2015-10-29

Comitter:

Gerth

Date:

Thu Oct 29 14:51:02 2015 +0000

Parent:

4:9492c1c361b9

Commit message:

corrected for deviation of angle

Changed in this revision

diff -r 9492c1c361b9 -r 2dc21bf3f3a0 angleandposition.cpp
--- a/angleandposition.cpp	Thu Oct 29 14:03:25 2015 +0000
+++ b/angleandposition.cpp	Thu Oct 29 14:51:02 2015 +0000
@@ -2,12 +2,14 @@
 #include "mbed.h"
 #include "math.h"
 
+
+const double correctionforupperarm= acos((0.255/0.260)); 
 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));
+const double virt_bar_ref = sqrt(pow(bar2,2)- pow(bar1,2));
 
 
 angleandposition::angleandposition(void)
@@ -16,7 +18,7 @@
 }
 
 double angleandposition::positiontoangle1(double x_position, double y_position)//rigth arm
-{
+{    
     double virt_bar_right = sqrt( pow(x_position-0.5*base_spacing,2) + pow(y_position,2) );
 
     double d2_square = pow(bar1,2) - pow( ( pow(virt_bar_right,2) + pow(bar1,2) - pow(bar2,2) ) / (-2*virt_bar_right) ,2);
@@ -30,7 +32,7 @@
         phi = asin(d2/bar1);
     }
     double theta_r = -phi + acos((x_position - 0.5*base_spacing)/virt_bar_right);
-    return theta_r;
+    return theta_r-correctionforupperarm;
 }
 
 double angleandposition::positiontoangle2(double x_position, double y_position)//left arm
@@ -51,11 +53,13 @@
 
     double theta_l = pi - phi - acos((x_position + 0.5*base_spacing)/virt_bar_left);
 
-    return theta_l;
+    return theta_l-correctionforupperarm;
 }
 
 double angleandposition::angletoposition(double theta_r,double theta_l)
 {
+    theta_r+=correctionforupperarm;
+    theta_l+=correctionforupperarm;
 
     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;
@@ -79,6 +83,9 @@
 
 double angleandposition::angletoposition_y (double theta_r, double theta_l)
 {
+    theta_r+=correctionforupperarm;
+    theta_l+=correctionforupperarm;
+    
     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));