Diff: angleandposition.cpp

Revision:

1:6cace9fdb088

Parent:

0:b8295c4b5793

Child:

2:b99fef8df97e

--- a/angleandposition.cpp	Thu Oct 15 11:19:39 2015 +0000
+++ b/angleandposition.cpp	Mon Oct 19 13:25:40 2015 +0000
@@ -1,43 +1,55 @@
 #include "angleandposition.h"
 #include "mbed.h"
 #include "math.h"
-#include <vector>
-using std::vector;
-
-Serial comp(USBTX,USBRX);
 
-const float bar1=260; // length bar 1 in mm
-const float bar2=342; // length bar 2 in mm
-const float y_position=225;// y positon of pod in mm
-const float y_punch=425;// y position when punched out in mm
-const float width_playfield=473; //width in mm
-const float base_spacing=105.29; //spacing between two dc motors at base
-const float number_steps=10;
-// make linspace of y positions
+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 pi = 3.1415926535897;
+
+double virt_bar_ref = sqrt(pow(bar2,2)- pow(bar1,2));
 
 
 angleandposition::angleandposition(void)
 {
-    float y_step=y_position;
-    vector<double> array;
-    double step = (y_punch-y_position) / (number_steps-1);
+
+}
+
+float angleandposition::positiontoangle1(float x_position, float 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);
 
-    while(y_step <= y_punch) {
-        array.push_back(y_step);
-        y_step += step;           // could recode to better handle rounding errors
-        comp.printf("y= %f\n",y_step);
+    double d2 = sqrt(d2_square);
+    
+    double phi;
+    if (virt_bar_right <= virt_bar_ref) {
+        double   phi = pi-asin(d2/bar1);
+    } else {
+        double phi = asin(d2/bar1);
     }
+    double theta_r = phi + acos((x_position - 0.5*base_spacing)/virt_bar_right);
+    return theta_r;
 }
 
-float angleandposition::positiontoangle1(float x_position)
+float angleandposition::positiontoangle2(float x_position, float y_position)//left arm
 {
-    /* float virt_bar_right = sqrt((x_const-0.5.*base_spacing).^2 + y.^2);
-     return angle1;*/
-}
+    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) {
+        double  phi = pi-asin(d2/bar1);
+    } else {
+        double phi = asin(d2/bar1);
+    }
 
-float angleandposition::positiontoangle2(float x_position)
-{
+    double theta_l = pi - phi - acos((x_position + 0.5*base_spacing)/virt_bar_left);
 
-
-    //return angle2;
+    return theta_l;
 }
\ No newline at end of file