kusano kiyoshige / Mbed 2 deprecated 17robo_fuzi

ライブラリ化を行った後

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_Practice1 by kusano kiyoshige

pid.h

Committer:
echo_piyo
Date:
2017-07-01
Revision:
7:c4ae1d001d09
Parent:
0:bf96e953cdb8

File content as of revision 7:c4ae1d001d09:

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;
};