File content as of revision 1:8c585244c1b0:

#include "mbed.h"
#include "Serial.h"
#include "QEI.h"
#include "math.h"


Serial      pc(USBTX, USBRX);        //Serial PC connectie
QEI encoder(D13, D12, NC, 32);       //encoder instellen
DigitalOut  motor1DirectionPin(D4);  //Motorrichting op D4 (connected op het bord)
PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
Ticker      controller;                //toets sample tijd
double fs = 10000;                     // sample frequentie
double ts = 1/fs;

// variablen voor void
int pulses;
int ref;
int er;
float Kp=0.0005;
float P;
float D;
float Dif;
float Kd=0.00012;
float PID;
int er2=0;

void PD()
{
char key;
    if(pc.readable()==true)  
    {   key = pc.getc();
        if (key=='q')
        {
        ref=-500;        //reference wordt 500 pulses
        }
        else if(key=='w')
        {
        ref=0;
        }
        else if(key=='e')
        {
        ref=500;          //reference wordt 0 pulses
        }
     }   
//error bepalen    
pulses=encoder.getPulses();
er=ref-pulses;

//PID
//Proportional part
P = Kp*er;

//Differential part
Dif=(er2-er)/ts;
D=Kd*Dif;

//PID sum
PID=P+D;
er2=er;

    //Motor control
    if (P<0)
    {
    motor1MagnitudePin = fabs(P);
    motor1DirectionPin = 1;
    }
    else if (P>0)
    {
    motor1MagnitudePin = fabs(P);
    motor1DirectionPin = 0;
    }

}

int main()
{
controller.attach_us(&PD, 10000);
    
while(true)
{
}

    
}