File content as of revision 3:123f3fd0daf6:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"
#include "time.h"

MODSERIAL pc(USBTX,USBRX);
Encoder Enc(PTA1,PTA2);
PwmOut motorplus(PTD3);
PwmOut motormin(PTC3);

double rpm, pos, pos1, t, Ref, Error, deliver;
double Prop, Kp, Int, Int1, Ki, Ctrl;
double pwmotor;
double speed, move;
volatile bool looptimerflag, calflag, scriptflag;
double Move[25];
double Tijd[9];
double Referentie[9];
int n;

void setlooptimerflag(void)
{
    looptimerflag = true;
}
void setscriptflag(void)
{
    scriptflag = true;
}
void setcalflag(void)
{
    calflag = true;
}
void printdata(void)
{
    pc.printf("Speed %f Pos %f del %f\n\r",speed, pos, deliver);
}
void printdata2(void)
{
    pc.printf("Del %f Ctrl %f\n\r",deliver,Ctrl);
}
void printdata3(void)
{
    pc.printf("P %f R %f\n\r",pos, Ref);
}

int main()
{
    pc.baud(115200);
    wait(1);
    pc.printf("start\n\r");
    wait(1);
    //Inladen data. Courtesy of http://runnable.com/UpSpP5ymsslIAAFo/reading-a-file-into-an-array-for-c%2B%2B-and-fstream

    //Nu gaan we weer verder met het normale programma
    Ticker looptimer;
    Ticker printer;
    Ticker script;
    Ticker calibratie;
    double ts=(1.0/5000.0);
    double tsd=(1.0/5.0);
    motorplus.period(1.0/60000.0);
    motormin.period(1.0/60000.0);

    Kp = 0.5;
    Ki = 0.4;
    pwmotor=-0.9/2.0;
    deliver=0.01*pwmotor;
    pos1=0;
    n=0;
    t=0;
    Move[24]=pwmotor*-100.0;
    pos=Enc.getPosition();
    speed=Enc.getSpeed();

    Referentie[0]=20.0;
    Referentie[1]=17.0;
    Referentie[2]=14.0;
    Referentie[3]=20.0;
    Referentie[4]=16.0;
    Referentie[5]=20.0;
    Referentie[6]=14.0;
    Referentie[7]=25.0;
    Referentie[8]=23.0;

    Tijd[0]=0;
    Tijd[1]=5.5;
    Tijd[2]=7.0;
    Tijd[3]=9.25;
    Tijd[4]=12.0;
    Tijd[5]=15.5;
    Tijd[6]=17.5;
    Tijd[7]=23.0;
    Tijd[8]=25.0;

    wait(1);
    pc.printf("start2\n\r");
    wait(1);

    scriptflag = true;
    printer.attach(printdata,tsd);
    //script.attach(setscriptflag, ts);
    calibratie.attach(setcalflag,ts);
    while(scriptflag == true) {
        while(calflag == true) {
            calflag=false;
            if (abs(deliver) <= abs(pwmotor)) {
                deliver=deliver*1.01;
            }
            motorplus.write(0.5+deliver);
            motormin.write(0.5-deliver);
            //pc.printf("pwm %f\n\r",deliver);
            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];
            pc.printf("Pwm %f Pos %f\n\r",deliver,pos);
            if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 0.5) {
                motorplus.write(0.5);
                motormin.write(0.5);
                //pwmotor = pwmotor*-1.0;
                //deliver=0.01*pwmotor;
                pc.printf("Einde\n\r");
                calibratie.detach();
                printer.detach();
                calflag=false;
                scriptflag=false;
                wait(1);
            }
        }
    }
    Enc.setPosition(0.0);
    pwmotor=0.0;
    deliver=0.01;
    pc.printf("Begin loop\n\r");
    wait(1);
    printer.attach(printdata3,tsd);
    looptimer.attach(setlooptimerflag,ts);
    Ref=Referentie[0];
    while(1) {
        while (looptimerflag != true) {}
        looptimerflag = false;
//Encoder
        pos=Enc.getPosition()/1024;
        speed=Enc.getSpeed();

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

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

        pwmotor=Ctrl;
        deliver=pwmotor;
        //if (abs(deliver) <= abs(pwmotor)) {
        //    deliver=deliver*1.01;
        //}
        //if (abs(deliver) >= abs(pwmotor)) {
        //    deliver=deliver*0.99;
        //}
        //pc.printf("del %f pwm %f\n\r",deliver,pwmotor);
        motorplus.write(0.5+deliver);
        motormin.write(0.5-deliver);
        if (t >= 30.0) {
            motorplus.write(0.0);
            motormin.write(0.0);
            pc.printf("Beindigd door tijd");
            return 0;
        }
        if (abs(pos) >= 30 || (t >=10.0 && pos <=0.5)) {
            motorplus.write(0.0);
            motormin.write(0.0);
            pc.printf("Beindigd door positie");
            return 0;
        }
    }
}