File content as of revision 9:24c0d83dec3c:

# define M_PI           3.14159265358979323846  /* pi */

#include "math.h"
#include "stdio.h"
#include "iostream"

// Constantes
//L1 = 0.35;
//L2 = 0.30;

// Variables
double V_q1;

double invj00;
double invj01;
double invj10;
double invj11;

double vx;
double vy;

double T;

double q1_new;

//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)

void IK(double &q1, double &q2, double vx, double vy, double T) {
    
    double Q2;
    double Q2_new;
    double V_Q2;

    Q2 = q2 + 90*(M_PI/180);

    invj00 = sin(q1 + Q2)/(0.35*cos(q1 + Q2)*sin(q1) - 0.35*sin(q1 + Q2)*cos(q1));
    invj01 = -cos(q1 + Q2)/(0.35*cos(q1 + Q2)*sin(q1) - 0.35*sin(q1 + Q2)*cos(q1));
    invj10 = -(0.3*sin(q1 + Q2) + 0.35*sin(q1))/(0.35*0.3*cos(q1 + Q2)*sin(q1) - 0.35*0.3*sin(q1 + Q2)*cos(q1));
    invj11 = (0.3*cos(q1 + Q2) + 0.35*cos(q1))/(0.35*0.3*cos(q1 + Q2)*sin(q1) - 0.35*0.3*sin(q1 + Q2)*cos(q1));
 
    V_q1 = invj00*vx + invj01*vy;
    V_Q2 = invj10*vx + invj11*vy;
    
    // Numerical Integral to make it position controlled
    q1_new = q1 + V_q1*T;
    q1 = q1_new;
    Q2_new = Q2 + V_Q2*T;
    Q2 = Q2_new;

    q2 = Q2 - 90*(M_PI/180);
}


int main(){
    double q1 = 0;
    double q2 = 0;
    double i;

        for(i = 0; i <= 30; i++)
            {
                IK(q1,q2, 0.05, 0, 0.1);
                printf("q1 = %f, q2 = %f\n",q1,q2);
            }
}