File content as of revision 2:7993edc4dba6:

# include "mbed.h"
# include "QEI.h"
# include "HIDScope.h"

// Encoder definieren
QEI Encoder1(D12,D13,NC,32); // Je gebruikt hierbij X2 encoding
QEI Encoder2(D14,D15,NC,32); // Je gebruikt hierbij x2 encoding

// Aantal pulses/deg
const double P = 4200/360;  //pulses/degree
const double L1 = 220.4;    //lengte arm 1
const double L2 = 130;      //lengte arm 2
const double R = 2.166;     //ratio van de tandwielen op de arm


double C1;      //aantal pulses motor 1
double C2;      //aantal pulses motor 2
double Q1;      //hoek motor 1
double Q2;      //hoek motor 2
double X;       
double Y;

// HIDScope definieren
HIDScope scope (2);
Ticker Kinematicatimer;

// Code voor de X- en Y-waarden
void Kinematica ()
{
    C1 = Encoder1.getPulses();  //Aantal pulsen gemeten motor 1
    C2 = Encoder2.getPulses();  //Aantal pulsen gemeten motor 2
    Q1 = C1/P;  // Hoek van motor 1
    Q2 = R*C2/P;  // Hoek van motor 2, met ratio van de tandwielen bovenop
    X = cos (Q1) * L1 + cos (Q1+Q2) * L2; //X waarde van einde arm 2    
    Y = sin (Q1) * L1 + sin (Q1+Q2) * L2; //Y waarde van einde arm 2   


    scope.set (0, X);
    scope.set (1, Y);
    scope.send ();
}

int main ()
{
    Kinematicatimer.attach_us(Kinematica, 1e4);
    while(true) {}
}