File content as of revision 0:095ff84c3ee9:

#include "mbed.h"
#include "encoder.h"

Serial pc(USBTX,USBRX);
Encoder Enc(PTA1,PTA2);
Encoder Encinv(PTA0,PTD0);
PwmOut motorplus(PTD3);
PwmOut motormin(PTC3);

double rpm, rpmmin,pos,pos1,posinv;
volatile double pwmotor;
double tickinv, tick, speed,move, speedinv;
volatile bool looptimerflag, calflag, scriptflag;
double Move[25];
int n;

void setlooptimerflag(void)
{
    looptimerflag = true;
}
void setscriptflag(void)
{
    scriptflag = true;
}
void printdata(void)
{
    pc.printf("Speed %f Pos %f\n\r",speed, pos);
}

int main()
{
    Ticker looptimer;
    Ticker printer;
    Ticker script;
    const double ts=(1.0/5000.0);
    const double tsd=(1.0/5.0);
    //20 kHz kan hij nog aan, in een simpel script.
    motorplus.period(1.0/60000.0);
    motormin.period(1.0/60000.0);
    //Die moet duidelijk gefilterd worden.
    pc.baud(115200);

    pwmotor=0.3/2.0;
    pos1=0;
    n=0;
    Move[24]=-10;
    calflag = true;

    printer.attach(printdata,tsd);
    wait(tsd);

    script.attach(setscriptflag, ts);
    while(scripttflag == true) {
        scriptflag = false;
        wait(3);
        while(calflag == true) {
            motorplus.write(0.5+pwmotor);
            motormin.write(0.5-pwmotor);
            speed=Enc.getSpeed();
            pos1=pos;
            pos=Enc.getPosition()/1024.0;
            Move[n]=pos-pos1;
            n += 1;
            if (n >24) {
                n=0;
            }
            move=Move[24]-Move[0];
            if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 10) {
                motorplus.write(0.0);
                motormin.write(0.0);
                pwmotor = pwmotor*-1;
            }
        }
        script.detach();
    }

    looptimer.attach(setlooptimerflag,ts);
    while(looptimerflag == false);
    {
        motorplus.write(0.5+pwmotor);
        motormin.write(0.5-pwmotor);
    }
    looptimerflag = false;

    //Encoder
    tick=Enc.getPosition();
    //tickinv=Encinv.getPosition();
    speed=Enc.getSpeed();
    //speedinv=Encinv.getSpeed();
    pos=tick/1024;
    //posinv=tickinv/1024;

    t += ts;
    if (t==Tijd[n]) {
        Ref=Referentie[n];
        n +=1;
    }

    //Regelaar
    Error=Ref-pos;
    Prop=Error*Kp;
    Int=(Int1+ts*Error)*Ki;
    Ctrl=Prop+Int;
    pwmotor=Ctrl;

    motorplus.write(0.5+pwmotor);
    motormin.write(0.5-pwmotor);
    //pc.printf("RPM %f Y %f\n\r",rpm, y);
}