File content as of revision 1:11b728aa0077:

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

// Define variables 
// Lengtes zijn in meters
// Vaste variabelen:
const double L0 = 0.088;                                  // Afstand achterkant robot tot joint 2
const double L1 = 0.288;                                  // Hoogte van tafel tot joint 2
const double L2 = 0.208;                                  // Hoogte van tafel tot joint 1
const double L3 = 0.212;                                  // Lengte van de arm
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_ticker;                          // 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 


int main() {
    while (true) {
        
        ticker.attach(ref_ticker);
        
        Lq1 = q1*r_trans;                            
        Cq2 = q2/5.0;                               

        q1_dot = (v_x + (v_y*(L2 + L3*sin(q2/5.0)))/(L0 + q1*r_trans + L3*cos(q2/5.0)))/r_trans;     
        q2_dot = (5.0*v_y)/(L0 + q1*r_trans + L3*cos(q2/5.0));                                       

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