File content as of revision 0:210b81f6c5b6:

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

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

double deliver, ts, pos, pos1, move;
volatile bool flag, loop;
double Move[25];
int n;

void setflag(void)
{
    flag=true;
}


int main()
{
    pc.baud(115200);

    Ticker Loop;

    ts=1.0/5000.0;

    motorplus.period(1.0/60000.0);
    motormin.period(1.0/60000.0);
    flag=true;
    loop=true;
    wait(3);
    Loop.attach(setflag,ts);
    while(loop==true) {
        while(flag !=true) {
        }
        flag=false;
        motorplus.write(-deliver);
        motormin.write(deliver);
        pos=Enc.getPosition()/1024.0;
        Move[n]=pos-pos1;
        n += 1;
        if (n >24) {
            n=0;
        }
        move=Move[24]-Move[0];
        if (abs(move)>0.002) {
            loop=false;
            motorplus.write(0.0);
            motormin.write(0.0);
            pc.printf("Fr %f\n\r",deliver);
            return 0;
        }
        pc.printf("del %f mo %f\n\r",deliver, move);
        
        deliver=deliver+ts/10;

    }



}