File content as of revision 0:d7e19af20f93:

#include "AnglePosition.h"
#include "mbed.h"

const double PI = 3.14159265358979323846;

AnglePosition::AnglePosition(void)
{
    
}

float AnglePosition::gettargetposition(double input, int max_range){
    float target = input * max_range;
    return target;
}


float AnglePosition::getreferenceposition(float target, float offset) {
    float  referenceposition = target + offset;
    return referenceposition;
}


float AnglePosition::getalpha(float max_rangex, float max_rangey, float x_offset, float y_offset, float alpha_offset, float L1, float L2, double inputx, double inputy) {
    float x_target = gettargetposition(inputx, max_rangex);
    float y_target = gettargetposition(inputy, max_rangey);
    float x = getreferenceposition(x_target, x_offset);
    float y = getreferenceposition(y_target, y_offset);        
    float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
    float alpha = alpha_ + alpha_offset;
    return alpha;
}
 
    
float AnglePosition::getbeta(float max_rangex, float max_rangey, float x_offset, float y_offset, float beta_offset, float L1, float L2, double inputx, double inputy) {
    float x_target = gettargetposition(inputx, max_rangex);
    float y_target = gettargetposition(inputy, max_rangey);
    float x = getreferenceposition(x_target, x_offset);
    float y = getreferenceposition(y_target, y_offset);    
    float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
    float beta = acos( ((L1*L1) + (L2*L2) - (x*x) - (y*y)) / (2*L1*L2)) - alpha_ + beta_offset;
    return beta;
}