File content as of revision 35:113a4015d408:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"
#define  M_PI 3.14159265358979323846  /* pi */
#include <math.h>
#include "Servo.h"
#include <cmath>
#include <complex>

Serial pc(USBTX, USBRX);

PwmOut motor1(D6);  // Misschien moeten we hiervoor DigitalOut gebruiken, moet
PwmOut motor2(D5);  // samen kunnen gaan met de servo motor

DigitalOut motor1_dir(D7);
DigitalOut motor2_dir(D4);

QEI Encoder1(D13, D12, NC, 64);
QEI Encoder2(D10, D9, NC, 64);

// VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
double pulses_M1;
double pulses_M2;
double counts1;
double counts2;
const double conversion_factor = (2.0*M_PI)/(64.0*131.25*2.0);
double theta_h_1_rad;
double theta_h_2_rad;
double offset1 = 0.0;
double offset2 = 0.0;

HIDScope scope(1);

Ticker loop;

void motors()
{
    motor1.write(0.6);
    motor1_dir.write(1);
    motor2.write(0.6);
    motor2_dir.write(1);
        
    // Berekenen van de motorhoeken (in radialen)
    pulses_M1 = Encoder1.getPulses();
    counts1 = pulses_M1*2;
    theta_h_1_rad = conversion_factor*(counts1);
    
    scope.set(0, theta_h_1_rad);
    scope.send();
}

int main(void)
{
    pc.printf("Opstarten\r\n");
    motor1.period_us(56);
    motor2.period_us(56);
    loop.attach(&motors, 0.002f);
    while(true) 
    {
    }
}