File content as of revision 10:546ff4637306:

#include <math.h>
#include "mbed.h"
#include "compute.h"

using namespace std;

const double k = 18.77;
const double l = 26.72;
const int Ax = -7;
const int Ay = 0;
const int Bx = 7;
const int By = 0;
const double upper_arm_lim = 170*(M_PI/180);
const double lower_arm_lim = 0;

double calcGamma(double ApBp) {
    return acos(((pow(ApBp,2))/(2*l*ApBp)));
}

double calcOmega(double Apx, double Apy, double Bpx, double Bpy) {
    return atan((Bpy-Apy)/(Bpx-Apx));
}

/* 
    Calculate Px in a different frame. This is the first row of the homogenous matrix (below) to calculate Px:
    [cos(omega)     -sin(omega)     Ax]
    [sin(omega)     cos(omega)      Ay]
    [0              0               1 ]

    Input:  omega (double)   = the angle omega
            fPx (double)     = x-coordinate of P in frame 1
            fPy (double)     = y-coordinate of P in frame 1
            Apx (double)     = A'x
    Output: Px in different frame (double)
*/
double homogenousX(double omega, double fPx, double fPy, double Apx) {
    return (cos(omega)*fPx) - (sin(omega)*fPy) + Apx;
}

/* 
    Calculate Py in a different frame. This is the second row of the homogenous matrix (below) to calculate Py:
    [cos(omega)     -sin(omega)     Ax]
    [sin(omega)     cos(omega)      Ay]
    [0              0               1 ]

    Input:  omega (double)   = the angle omega
            fPx (double)     = x-coordinate of P in frame 1
            fPy (double)     = y-coordinate of P in frame 1
            Apy (double)     = A'y
    Output: Py in different frame (double)
*/
double homogenousY(double omega, double fPx, double fPy, double Apy) {
    return (sin(omega)*fPx) + (cos(omega)*fPy) + Apy;
}

/*
    Calculate the angle between two points A and B
*/
double calcTanDifference(double Ax, double Ay, double Bx, double By) {
    return atan2((Ay-By), (Ax-Bx));
}

double calcLengthPoints(double Ax, double Ay, double Bx, double By) {
    return sqrt(pow(Ax-Bx,2) + pow(Ay-By,2));
}

double calcCosAngle(double line) {
    return acos((pow(line,2) + pow(k,2) - pow(l,2)) / (2*k*line));
}

double calcTanDirected(double Ax, double Ay, double Bx, double By, double Ox, double Oy) {
    double rads = atan2((Ay-Oy), (Ax-Ox)) - atan2((By-Oy), (Bx-Ox));
    return (rads > 0 ? rads : (2*M_PI+rads));
}

bool checkArm(double arm) {
    return (arm < upper_arm_lim && arm > lower_arm_lim);
}

int rad2deg(double rad) {
    return floor(rad*180);
}

double deg2rad(double deg) {
    return (deg/180);
}

/*
    Calculate the position of Point P given two angles, alpha and beta.
    Input:  alpha (int) = angle alpha
            beta (int)  = angle beta
            double& Px, Py = needed to change Px, Py
    Output: -
*/
void Angles2Point(double alpha, double beta, double& Px, double& Py) {
    alpha = alpha*M_PI;
    beta = beta*M_PI;
    
    // Calculate A' and B' (x and y coordinate seperately)
    double Apx = k*cos(alpha)+Ax;
    double Apy = k*sin(alpha)+Ay;
    double Bpx = k*cos(beta)+Bx;
    double Bpy = k*sin(beta)+By;

    double ApBp = calcLengthPoints(Apx, Apy, Bpx, Bpy);

    // Calculate gamma
    double gamma = calcGamma(ApBp);
    
    // Calculate Px and Py in frame 1
    double fPx = l*cos(gamma);
    double fPy = l*sin(gamma);
    
    // Calculate omega, angle between frame 0 and frame 1
    double omega = calcOmega(Apx, Apy, Bpx, Bpy);
    
    // Calculate Px and Py in frame 0 and update it
    Px = homogenousX(omega, fPx, fPy, Apx);
    Py = homogenousY(omega, fPx, fPy, Apy);

    // Check physical constraints
    double armA = calcTanDirected(Px, Py, Ax, Ay, Apx, Apy);
    double armB = calcTanDirected(Bx, By, Px, Py, Bpx, Bpy);
    if(!(checkArm(armA) && checkArm(armB))) {
        return;
    }

}


void Point2Angles(double Px, double Py, double& a, double& b) {
    // Calculate gamma of the arm A and arm B
    double gamma_a = calcTanDifference(Px, Py, Ax, Ay);
    double gamma_b = calcTanDifference(Px, Py, Bx, By)*(-1);
    
    // Calculate |AP| and |BP|
    double AP = calcLengthPoints(Px, Py, Ax, Ay);
    double BP = calcLengthPoints(Px, Py, Bx, By);


    // Calculate omega of the arm A and arm B
    double omega_a = calcCosAngle(AP);
    double omega_b = calcCosAngle(BP);

    // Calculate A'(A'x, A'y) and B'(A'x, A'y) in the new frame
    double Apx2 = k*cos(omega_a);
    double Apy2 = k*sin(omega_a);
    double Bpx2 = k*cos(omega_b);
    double Bpy2 = k*sin(omega_b);

    // Translate and rotate A' and B' from frame 2 to frame 1 using gamma.
    double Apx = homogenousX(gamma_a, Apx2, Apy2, Ax);
    double Apy = homogenousY(gamma_a, Apx2, Apy2, Ay);
    double Bpx = homogenousX(gamma_b, Bpx2, Bpy2, Bx);
    double Bpy = homogenousY(gamma_b, Bpx2, Bpy2, By);

    
    // Calculate alpha and beta
    a = calcTanDifference(Apx, Apy, Ax, Ay);
    b = calcTanDifference(Bpx, Bpy, Bx, By)*(-1);
    
    // If alpha < 0, make alpha positive
    if(a<0) {
        double alpha = (2*M_PI)+a;
        a = alpha;
    }

    // Check physical constraints
    double armA = calcTanDirected(Px, Py, Ax, Ay, Apx, Apy)/M_PI;
    double armB = calcTanDirected(Bx, By, Px, Py, Bpx, Bpy*(-1))/M_PI;
    if(armA*180>170 || armB*180>170) {
        a = 100;
        b = 100;
    }
    
    a = a/M_PI;
    b = b/M_PI;
}

int compute_max(char dir, int pos) {
    double a,b;
    double high = (dir=='X') ? 35 : 44;
    double cur = (high/2);
    double low = 0;
    while(abs(high-low)>0.00001) {
        if(dir=='X') Point2Angles(cur, pos, a, b);
        else Point2Angles(pos, cur, a, b);
        if(a < 31 && b < 31) {
            low = cur;
            cur = (high+low)/2;
            if(cur>high) cur = high;
        } else {
            high = cur;
            cur = (high+low)/2;
            if(cur<low) cur = low;
        }
    }
    return (dir == 'X') ? low : (low-1);
}