File content as of revision 6:0403c2e276c2:

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

#define K_P (0.1)
#define K_I (0.00001)
#define K_D (0.0005 /TSAMP)
#define TSAMP 0.001
#define I_LIMIT 1.
#include <iostream>

Encoder encoderA(PTD0,PTD2);

PwmOut pwm(PTA5);
DigitalOut dir(PTA4);


int32_t enc = 0,enca2 =0,enca1=0, encp=0, counts =0;
float speed = 0.1, out =0;
int pos =0,zero =0, fout;
float v=0;

void clamp(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;
}




float pid(float setpoint, float measurement)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    static float    out_i = 0;
    float           out_d = 0;
    error  = setpoint-measurement;
    out_p  = error*K_P;
    out_i += error*K_I;
    out_d  = (error-prev_error)*K_D;
    clamp(&out_i,-I_LIMIT,I_LIMIT);
    prev_error = error;
    out  = out_i;

    return out;
}

float getv(float delta_t)
{
    int n =0 ;
    while(n<3) {
        wait(delta_t);
        enc = encoderA.getPosition();
        enca2 = enca1;
        enca1 = enc;
        n++;

    }

    counts = (enca1 - enca2)/delta_t;
    v = (counts)*((2*3.14159265359)/1550);
    return v;
}

float reset()
{
    v = 1;
    while(v !=0) {

        dir = 0;
        pwm.write(0.1);
        v =getv(0.1);
    }
    pwm = 0;
    dir =1;
    encoderA.setPosition(0);
    zero = encoderA.getPosition();
    cout<<"zero encoder: "<<zero<<endl;
    return pwm;
}

float gotopos(float pos)
{
    enc = encoderA.getPosition();

    while((pos - enc)>0) {
        wait(0.1);
        enc = encoderA.getPosition();
        out = pid(pos, enc);
        pwm = out;
    }
    pwm =0;
    enc = encoderA.getPosition();
    cout<<"final enc: "<<enc<<endl;
    cout<<"final error: "<<error<<endl;
    return pwm;
}

int main()
{
    while(1) {
        reset();
        cout<<"typ pos: "<<endl;
        cin>>pos;
        gotopos(pos);
        wait(5);
    }
}