File content as of revision 2:9dc7b40286d1:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"
#include <iostream>
#include <fstream>
using namespace std;
#include "time.h"

MODSERIAL pc(USBTX,USBRX);
Encoder Enc(PTA1,PTA2);
//Encoder Encinv(PTA0,PTD0);
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,tr;

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

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.01;
    Ki = 0.01;
    pwmotor=-0.5/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]=14.0;
    Referentie[5]=20.0;
    Referentie[6]=14.0;
    Referentie[7]=29.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*pwmotor;
    pc.printf("Begin loop\n\r");
    wait(1);
    //printer.attach(printdata2,tsd);
    looptimer.attach(setlooptimerflag,ts);
    while(1) {
        while (looptimerflag != true) {
            tr = clock();
            looptimerflag = false;
            pc.printf("true\n\r");
//Encoder
            pos=Enc.getPosition()/1024;
            speed=Enc.getSpeed();

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

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

            pwmotor=Ctrl;
            if (abs(deliver) <= abs(pwmotor)) {
                deliver=deliver*1.01;
            }
            if (abs(deliver) >= abs(pwmotor)) {
                deliver=deliver*0.99;
            }
            motorplus.write(0.5+deliver);
            motormin.write(0.5-deliver);
            pc.printf("true2\n\r");
            //wait(1);
            //cout << double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " seconds." << endl;
            //cout << double( clock() - startTime ) << " seconds." << endl;
            tr =clock()-tr;
            pc.printf("cyc %d time %f \n\r",tr,((float)tr)/CLOCKS_PER_SEC);
        }
    }
}