File content as of revision 6:0256de2854d6:

#include "mbed.h"
#include "math.h"
#include "MODSERIAL.h"

Serial pc(USBTX, USBRX);

// Constantes
const double L1 = 0.35;
const double L2 = 0.30;
double T = 0;

double q1;
double q2;

double R1;
double R2;


//Hoe werken de variabls?
// q1 en q2 komen uit de Encoder dus die worden geleverd
// This is the function for the Inverse Kinematics We start with a inverse Jacobian so we can determine q_dot (rotation speed of the motors)

double IK(double q1, double q2, double vx, double vy) {
    
    double invj[2][2] =
    {
        {sin(q1 + q2)/(L1*cos(q1 + q2)*sin(q1) - L1*sin(q1 + q2)*cos(q1)), -cos(q1 + q2)/(L1*cos(q1 + q2)*sin(q1) - L1*sin(q1 + q2)*cos(q1))},
        {-(L2*sin(q1 + q2) + L1*sin(q1))/(L1*L2*cos(q1 + q2)*sin(q1) - L1*L2*sin(q1 + q2)*cos(q1)), (L2*cos(q1 + q2) + L1*cos(q1))/(L1*L2*cos(q1 + q2)*sin(q1) - L1*L2*sin(q1 + q2)*cos(q1))}
    };
    
    double V_q1 = invj[0][0]*vx + invj[0][1]*vy;
    double V_q2 = invj[1][0]*vx + invj[1][1]*vy;
    
    // Numerical Integral to make it position controlled
    double q1_new = q1 + V_q1*T;
    q1 = q1_new;
    double q2_new = q2 + V_q2*T;
    q2 = q2_new;
    
}


int main(){
    IK(0, 0, 0.05, 0);
    while(T<=3) {
        T = T + 0.01;
        IK(q1, q2, 0.05, 0);   
    }
    pc.baud(115200);
    pc.printf("q1 = %d, q2 = %d\n",q1,q2);
}