File content as of revision 1:79c4c5746a33:

#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, statloop,viscloop;
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;
    statloop=true;
    viscloop=true;
    wait(3);
    
    Loop.attach(setflag,ts);
    while(statloop==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) {
            statloop=false;
            motorplus.write(0.0);
            motormin.write(0.0);
            pc.printf("Fs %f\n\r",deliver);
            wait(1);
        }
        
        deliver = deliver+ts;

    }

    deliver=0.4;
    motorplus.write(-deliver);
    motormin.write(deliver);
    wait(1.0);
    while(viscloop==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.0) {
            viscloop=false;
            motorplus.write(0.0);
            motormin.write(0.0);
            pc.printf("Fv %f\n\r",deliver);
            return 0;
        }
        deliver = deliver-ts;

    }


}