File content as of revision 32:f535ace7c529:

#define PI      3.141592654f
#define Nmax    18000
#define invalidN    0.1

class Position_pid_enc {
public  :
    void set_parameter(float Kp, float Ki, float Kd) {
        kp = Kp;
        ki = Ki;
        kd = Kd;
    }

    void position_cal(float target, float nowval, float dt) {
        old = now;
        now = nowval - target;
        p = now;
        i = i + (now + old)/2.0f * dt;
        d = (now - old) / dt;
        result = kp*p + ki*i + kd*d;
        if (result > 1.0f) {
            result = 1.0f;
        } else if (result < -1.0f) {
            result = -1.0f;
        }
    }

    float duty() {
        return result;
    }

private :
    float kp, ki, kd,
          old, now,
          p, i, d, result;
};
/*
class Speed_pid
{
public:
    void set_parameter(float Kp, float Ki, float Kd) {
        kp = Kp;
        ki = Ki;
        kd = Kd;
    }
    
    float duty(){
        return result;
    }
    
    float e,p,i,d;

protected :
    float kp,ki,kd;
    float e1,e2;
    //float p,i,d;
    float result;
    
    void pid_cal(float target, float val, float dt){
        e2 = e1;
        e1 = e;
        e = target - val;
        p = e - e1;
        i = e;
        d = (e-2*e1+e2);
        result = result + (kp*p+ki*i+kd*d);
        if (result > 1.0f) {
            result = 1.0f;
        } else if (result < -1.0f) {
            result = -1.0f;
        }
        if ((target == 0) && (fabs(e) < invalidN)) {
            result = 0;
        }
    }  
};
*/
class Encoder : public Position_pid_enc //: public Speed_pid
{
public:
    Encoder (PinName A_Phase, PinName B_Phase) : A(A_Phase), B(B_Phase) {
    }

    void setup(int Ppr) {
        ppr = Ppr;
        A.rise(this, &Encoder::A_count);
        B.rise(this, &Encoder::B_count);
        origin();
    }
    
    void cal(float target, float dt){
        radian = (float) count/ppr*2*PI;
        degree = radian / PI * 180.0;
        w = (radian - oldtheta) / dt;
        v = 0.05*w/27;
        n = w*30/(PI);
        oldtheta = radian;
        //pid_cal(target, n/Nmax, dt);
        //deg_cylinder = (count / 1200) * 360 * 3 / 20;
        position_cal(target, (float)degree, dt);
    }

    float deg(){
        return degree;
    }
    
    float get_deg_cylinder(){
        return deg_cylinder;
    }
    
    float rad() {
        return radian;
    }
    
    long pulse(){
        return count;
    }
    
    float N(){
        return n;
    }

    void origin() {
        count = 0;
    }

private :
    InterruptIn A;
    InterruptIn B;

    int ppr;
    signed long count;
    float radian,oldtheta;
    float w,v,n;
    float degree, deg_cylinder;

    void A_count() {
        count ++;
    }

    void B_count() {
        count --;
    }
};