File content as of revision 2:44cc689e14d6:

#include "mbed.h"
#include "encoder.h"
#define K_P (0.1)
#define K_I (0.1)
#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);
int y1;



int32_t enca = 0,enca1 =0,enca2 =0,counts=0, encb;
float speed = 0,v=0,v_ref=0;
int n=0,a =1 ,b =0;
float out;


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;
    clamp(&out, -1,1);
    out = fabs(out);
    return out;
}

float getv()
{
    while(n<3) {
        wait(0.2);
        enca = encoderA.getPosition();
        enca2 = enca1;
        enca1 = enca;
        n++;
        cout<<n<<endl;
    }

    n =0 ;
    counts = (enca1 - enca2)/0.4;
    cout<<"counts: "<<counts<<endl;
    v = (counts)*((2*3.14159265359)/1550);
    return v;
}

float speedcontrol()
{

    v = getv();
    cout<<"v: "<<v<<endl;
    out = pid(v_ref,v);
    cout<<"out: "<<out<<endl;
    pwm = out;
}

int main()
{
    dir = 1;
    cout<<"typ case: "<<endl;
    cin>>y1;
    cout<<"case: "<<y1<<endl;
    

    while(1) {
        switch(y1) {
            case 0 :
                v_ref = 1;
                break;
            case 1 :
                v_ref = 2;
                break;
            case 2 :
                v_ref = 3;
                break;
        }

        speedcontrol();

    }
}