kusano kiyoshige / Mbed 2 deprecated 17robo_tokyo_kaede

sampleProgram

Dependencies:   QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo

Fork of 17robo_fuzi by kusano kiyoshige

pid.h

Committer:
echo_piyo
Date:
2017-06-26
Revision:
0:bf96e953cdb8
Child:
7:c4ae1d001d09
Child:
10:04f2a82cfd89

File content as of revision 0:bf96e953cdb8:

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

    void 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 setup(float Kp, float Ki, float Kd) {
        kp = Kp;
        ki = Ki;
        kd = Kd;
    }

    void cal(float target, float nowval, float dt) {
        e2 = e1;
        e1 = e;
        e = nowval - target;
        p = e - e1;
        i = e*dt;
        d = (e-2*e1+e2)/dt;
        result = 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,
          e, e1, e2,
          p, i, d, result;
};