File content as of revision 0:00721d3e1cf7:

#include "mbed.h"
#include <math.h> 
#include "Matrix.h"
// Define variables 
// Lengtes zijn in meters
double L0 = 0.088;                                  // Afstand achterkant robot tot joint 2
double L1 = 0.288;                                  // Hoogte van tafel tot joint 2
double L3 = 0.212;                                  // Lengte van de arm
// double r_trans;                                  // Kan gebruikt worden om om te rekenen van translation naar shaft rotation 
double q1;                                          // Translational joint, waarde uit encoder
double q2;                                          // Rotational joint, waarde uit encoder --> q2 is 1/5 * waarde vd encoder (omrekeningsfactor)
double v_x;                                         // Desired velocity end effector in x direction --> Determined by EMG signals
double v_y;                                         // Desired velocity end effecftor in y direction --> Determined by EMG signals

double J2_11 = 1.0;                                 // Berekeningen voor J'' komen uit MATLAB. [J2_11 J2_12; J2_21 J2_22] 
double J2_12 = -L3*sin(q2);
double J2_21 = 0.0; 
double J2_22 = L3*cos(q2); 
double T;                                           // Ticker value

double q_dot_1 = v_x +(v_y*sin(q2))/cos(q2);        // Deze waarde gaat naar de motor
double q_dot_2 = v_y/(L3*cos(q2)); 

double q_t_1 = L0 + q1 + L3*cos(q2); 
double q_t_2 = L1 + L3*sin(q2); 

double q_tT_1 = L0 + q1 + T*(v_x + (v_y*sin(q2))/cos(q2)) + L3*cos(q2);
double q_tT_2 = L1 + L3*sin(q2) + (T*v_y)/(L3*cos(q2));



int main() {
    while (true) {
        
    }
}