File content as of revision 1:95dd5c626960:

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

Serial 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;
volatile double pwmotor;
double tick, speed, move;
volatile bool looptimerflag, calflag, scriptflag, loadingflag;
double Move[25];
double Tijd[9];
double Referentie[9];
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()
{
    pc.baud(115200);
    wait(1);
    pc.printf("start\n\r");
    wait(1);
    loadingflag = true;
    //Inladen data. Courtesy of http://runnable.com/UpSpP5ymsslIAAFo/reading-a-file-into-an-array-for-c%2B%2B-and-fstream

    /*
        while(loadingflag == true) {
            int array_size = 2847;
            char * array = new char[array_size];
            int index = 0;
            ifstream file("/local/m3l5xascii.txt");
            if(file.is_open()) {
                cout << "Bestand geopend. Laden...";
                while(!file.eof() && index < array_size) {
                    file.get(array[index]);
                    index++;
                }
                array[index-1] = '\0';
                cout << "De data tot nu" << endl;
                for(int i = 0; array[i] != '\0'; i++) {
                    cout <<array[i];
                }
            } else {
                cout << "Bestand kan niet worden geopend" << endl;
            }
            loadingflag = false;
            cout << array;
            return 0;
        }
        */

    wait(1);
    //Nu gaan we weer verder met het normale programma
    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.

    Kp = 0.01;
    Ki = 0.01;
    pwmotor=-0.5/2.0;
    deliver=0.01*pwmotor;
    pos1=0;
    n=0;
    Move[24]=pwmotor*-100.0;

    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;

    printer.attach(printdata,tsd);
    script.attach(setscriptflag, ts);
    pos=Enc.getPosition();
    speed=Enc.getSpeed();
    //pc.printf("start2");
    wait(1);
    calflag = true;

    while(scriptflag == true) {
        scriptflag = false;
        wait(3);
        while(calflag == true) {
            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 move %f\n\r",deliver,move);
            if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 3) {
                motorplus.write(0.0);
                motormin.write(0.0);
                //pwmotor = pwmotor*-1.0;
                //deliver=0.01*pwmotor;
                calflag=false;
                pc.printf("Einde");
                script.detach();
                wait(1);
            }
        }
    }
    Enc.setPosition(0.0);
    looptimer.attach(setlooptimerflag,ts);
    pwmotor=0.0;
    while(looptimerflag == false);
    {
        motorplus.write(0.5+deliver);
        motormin.write(0.5-deliver);
    }
    looptimerflag = false;

//Encoder
    tick=Enc.getPosition();
    speed=Enc.getSpeed();
    pos=tick/1024;

    if (t==Tijd[n]) {
        Ref=Referentie[n];
        n +=1;
    }
    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);
}