File content as of revision 6:f36e6aaf64e2:

#include <iostream>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <fstream>
#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 float upper_arm_lim = 150*(M_PI/180);
const float lower_arm_lim = 0;
ofstream info_stream;

Serial  pc4(USBTX,USBRX);


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

float calcOmega(float Apx, float Apy, float Bpx, float 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 (float)   = the angle omega
            fPx (float)     = x-coordinate of P in frame 1
            fPy (float)     = y-coordinate of P in frame 1
            Apx (float)     = A'x
    Output: Px in different frame (float)
*/
float homogenousX(float omega, float fPx, float fPy, float 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 (float)   = the angle omega
            fPx (float)     = x-coordinate of P in frame 1
            fPy (float)     = y-coordinate of P in frame 1
            Apy (float)     = A'y
    Output: Py in different frame (float)
*/
float homogenousY(float omega, float fPx, float fPy, float Apy) {
    return (sin(omega)*fPx) + (cos(omega)*fPy) + Apy;
}

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

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

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

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

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

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

float deg2rad(float 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
            float& Px, Py = needed to change Px, Py
    Output: -
*/
void Angles2Point(float alpha, float beta, float& Px, float& Py) {
    alpha = alpha*M_PI;
    beta = beta*M_PI;
    
    // Calculate A' and B' (x and y coordinate seperately)
    float Apx = k*cos(alpha)+Ax;
    float Apy = k*sin(alpha)+Ay;
    float Bpx = k*cos(beta)+Bx;
    float Bpy = k*sin(beta)+By;

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

    // Calculate gamma
    float gamma = calcGamma(ApBp);
    
    // Calculate Px and Py in frame 1
    float fPx = l*cos(gamma);
    float fPy = l*sin(gamma);
    
    // Calculate omega, angle between frame 0 and frame 1
    float 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
    float armA = calcTanDirected(Px, Py, Ax, Ay, Apx, Apy);
    float 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
    float gamma_a = calcTanDifference(Px, Py, Ax, Ay);
    float gamma_b = calcTanDifference(Px, Py, Bx, By)*(-1);
    
    // Calculate |AP| and |BP|
    float AP = calcLengthPoints(Px, Py, Ax, Ay);
    float BP = calcLengthPoints(Px, Py, Bx, By);


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

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

    // Translate and rotate A' and B' from frame 2 to frame 1 using gamma.
    float Apx = homogenousX(gamma_a, Apx2, Apy2, Ax);
    float Apy = homogenousY(gamma_a, Apx2, Apy2, Ay);
    float Bpx = homogenousX(gamma_b, Bpx2, Bpy2, Bx);
    float 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) {
        float alpha = (2*M_PI)+a;
        a = alpha;
    }

    // Check physical constraints
    float armA = calcTanDirected(Px, Py, Ax, Ay, Apx, Apy)/M_PI;
    float 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 max(char dir, int pos) {
    double a,b;
    float high = (dir=='X') ? 35 : 44;
    float cur = (high/2);
    float 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);
}