File content as of revision 9:4743f3bb39b2:

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

DigitalOut Direction(D4); //1 = CCW - 0 = CW
PwmOut PowerMotor(D5); //van 0 tot 1
AnalogIn PotMeter(A1);
QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
Serial pc(USBTX, USBRX);
Ticker Pot;

double z=0;
const double twopi = 6.2831853071795;
const double pi = twopi/2;
int Pulses;
double Rotatie;
double Rotatietwopi;
double Goal = 0;
double Error = 0;
double K = 2;
double v;

void readpot()
{
    z = PotMeter.read()/10;
}


int main()
{
    pc.baud(115200);
    PowerMotor.write(0);
    Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
    while (true) {
        Pulses = Encoder.getPulses();
        Rotatie = (Pulses*twopi)/4192;
        Rotatietwopi = fmod(Rotatie,twopi);
        
        pc.printf ("Potmeter = %f\n", z);return 0; pc.printf ("Rotatie = %f [radialen] \n", Rotatietwopi);
//        if (z > 0.05) {
//            Goal = pi;
//        }
        Error = Goal-Rotatietwopi;
        if (Error >= 0) {
            Direction =0;
        } else {
                        Direction =1;
        }
        pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
        v = K*fabs(Error); 
        PowerMotor.write(v);
    }
}