File content as of revision 3:6aa73c8a0489:

#include "mbed.h"
#include <math.h>  //om dingen zoals sqrt te kunnen laten werken

// Define variables 
// Lengtes zijn in meters
// Vaste variabelen:
const double L1 = 0.05;                                   // Hoogte van base plaat tot joint 1 = afhankelijk van de patient
const double L2 = 0.05;                                   // Hoogte van base plaat tot joint 2 = dus als hij in de startpositie staat is dit dezelfde hoogte als L2
const double L3 = 0.30;                                   // Lengte van de arm 1
//const double L4 = 0.089;                                // Afstand van achterkant base tot joint 1
const double L5 = 0.30;                                   // Afstand van joint 1 naar joint 2 = arm 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);      
        }
}