File content as of revision 0:a204dfa75158:

#include "mbed.h"

volatile float q_1;
volatile float q_2;
volatile float r_1;
volatile float r_2;
volatile const float r_3 = 0.035;

int main()
{
    q_1= 1.05;
    q_2= 2.5;
    r_1= -0.2;
    r_2= -0.2;
        float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
        float z = 2.0*(r_2*cos(q_1)*cos(q_2))-r_3;
        float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0*(r_1*cos(q_1))-r_3;
        float x = (-2.0)*r_2*sin(q_1)*cos(q_2);
        float D =1.0/(u*z-x*y);                         // Determinant
        printf("Determinant is %f\n", D);
        
        float a = D*z;                                  // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
        float b = -D*x;                                 // Inverse jacobian
        float c = -D*y;                                 // Inverse jacobian
        float d = D*u;                                  // Inverse jacobian
        
        float vx = 0.01;                                // uit emg data
        float vy = 0.0;                                 // uit emg data
        float w_1 = vx*a+vy*b;
        float w_2 = vx*c+vy*d;
        printf("%f\n", w_1);
        printf("%f\n", w_2);
        
 /*       printf("a is%f\n", a);
        printf("%f\n", b);
        printf("%f\n", c);
        printf("%f\n", d);
*/                
 /*   float Adj_jac [2][2];
    Adj_jac[0][0] = 2.0;
    Adj_jac[0][1] = 3.0;
    Adj_jac[1][0] = 4.0;
    Adj_jac[1][1] = 5.0;
    printarray("%f", Adj_jac);
*/ 

}