File content as of revision 4:022b69da83f8:

#include "mbed.h"
#include "MODSERIAL.h"
#define PI 3.14159265

MODSERIAL pc(USBTX, USBRX);

void DirectKin(double& a, double& b, double theta1, double theta2)
{
    double L1 = 0.34;
    double L2 = 0.26;
    double theta1_0 = 0;
    double theta2_0 = 180;

    double r0_1[2] = {L1*cos((theta1_0+theta1) * PI / 180.0) , L1*sin((theta1_0+theta1) * PI / 180.0)};
    double r0_2[2] = {L1*cos((theta1_0+theta1) * PI / 180.0)+L2*cos((theta1_0+theta1+theta2+theta2_0)* PI / 180.0), L1*sin((theta1_0+theta1) * PI / 180.0)+L2*sin((theta1_0+theta1+theta2+theta2_0)* PI / 180.0)};

    double x = r0_2[0];
    double y = r0_2[1];

    a = x;
    b = y;
}

void IndirectKin(double&a, double& b, double x, double y)
{
    double L1 = 0.34;
    double L2 = 0.26;
    double theta1_0 = 30;
    double theta2_0 = 180;
    
    double theta_2 = acos( (x*x+y*y-L1*L1-L2*L2)/(2*L1*L2) )* 180 / PI;
    double theta_total = atan2 (y,x) * 180 / PI;
    double theta_Q = acos( (x*x+y*y+L1*L1-L2*L2) / (2*L1*sqrt(x*x+y*y)) )* 180 / PI;
    double theta_1 = theta_total - theta_Q;

    a = theta_1 - theta1_0;
    b = theta_2 - theta2_0;
}


int main()
{
    pc.baud(115200);

    double theta_1 = 0.000001;
    double theta_2 = 0.000001;

    double xy[2] = {0.0,0.0};



    while (true) {
        wait(0.5f);
        DirectKin(xy[0],xy[1], theta_1, theta_2);
        pc.printf("%4.4f %4.4f \n",xy[0], xy[1]);

        IndirectKin(theta_1, theta_2,  xy[0], xy[1]);
        pc.printf("%4.4f %4.4f \n\n",theta_1, theta_2);

        theta_1 = theta_1 + 90.0;
    }
}