File content as of revision 5:31120c4c08f0:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"
#include "time.h"
#include "ref.h"
#include "Tijd.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, size;
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 E %f\n\r",pos, Ref,Error);
}
void printdata4(void)
{
    pc.printf("%f\n",Error);
}

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 = 1.1471/1;
    //Ki = 4.9157/1;
    Kp=1.5;
    Ki=0.5;
    pwmotor=-0.9/2.0;
    deliver=0.01*pwmotor;
    pos1=0;
    n=0;
    size=sizeof(Referentie)/sizeof(float);
    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(printdata4,tsd);
    looptimer.attach(setlooptimerflag,ts);
    Ref=Referentie[0];
    while(1) {
        while (looptimerflag != true) {}
        looptimerflag = false;
//Encoder
        pos=Enc.getPosition()/1024.0;
        speed=Enc.getSpeed();

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

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

        pwmotor=Ctrl;
        deliver=pwmotor;
        motorplus.write(0.5+deliver);
        motormin.write(0.5-deliver);
        if (t >= size/100) {
            motorplus.write(0.0);
            motormin.write(0.0);
            pc.printf("\n\rBeindigd door tijd");
            return 0;
        }
        if (abs(pos) >= 30 || (t >=5.0 && pos <=0.5)) {
            motorplus.write(0.0);
            motormin.write(0.0);
            pc.printf("\n\rBeindigd door positie\n\r");
            return 0;
        }
    }
}