File content as of revision 2:8eca67634cba:

#include "mbed.h"
#include <math.h> 

// Define variables 
// Lengtes zijn in meters
// Vaste variabelen:
const double L1 = 0.208;                                  // Hoogte van tafel tot joint 1
const double L2 = 0.288;                                  // Hoogte van tafel tot joint 2
const double L3 = 0.212;                                  // Lengte van de arm
const double L4 = 0.089;                                  // Afstand van achterkant base tot joint 1
const double L5 = 0.030;                                  // Afstand van joint 1 naar joint 2
const double r_trans = 0.035;                             // Kan gebruikt worden om om te rekenen van translation naar shaft rotation 
const double T = 0.002f;                                  // Ticker value
Ticker ref_tick;                                          // Ticker aanmaken 


// Variërende variabelen: 
double q1 = 0;                                      // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
double q2 = 0;                                      // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
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 Lq1;                                         // Translatieafstand als gevolg van motor rotation joint 1
double Cq2;                                         // Joint angle of the system (corrected for gear ratio 1:5)

double q1_dot;                                      // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
double q2_dot;                                      // Benodigde hoeksnelheid van motor 2 om v_des te bereiken 

double q1_ii;                                       // Reference signal for motorangle q1 
double q2_ii;                                       // Reference signal for motorangle q2 


void inverse_kinematics{
        Lq1 = q1*r_trans;                            
        Cq2 = q2/5.0;                               

        q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));     
        q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));                                       

        q1_ii = q1 + q1_dot*T;                       
        q2_ii = q2 + q2_dot*T; 
        
        q1 = q1_ii;
        q2 = q2_ii; 
        }


int main() {
    while (true) {
        
        ref_tick.attach(&inverse_kinematics,T);      
        
    }
}