File content as of revision 3:067c75c62882:

#include "angleandposition.h"
#include "mbed.h"
#include "math.h"

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));


angleandposition::angleandposition(void)
{

}

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);

    double d2 = sqrt(d2_square);

    double phi;
    if (virt_bar_right <= virt_bar_ref) {
        phi = pi-asin(d2/bar1);
    } else {
        phi = asin(d2/bar1);
    }
    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 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_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;
}