倒立振子に挑戦 (2)

制御はPIDを使う。
普段組み込まれたPIDは作った事はあるが、プログラムした事はない。

こちらを参考にお手軽版でクラスを起こした。

controlabo.com

(追記: ちょっと修正)

class Pid {
  private:
    // aim
    float r;

    // input
    float kp;
    float ki;
    float kd;
    float freq;

    // var
    float t;
    float e_pre;   // error
    float e;    // error
    float de;
    float ie;

    // output
    float u;

  public:

    Pid(float aim, float kp, float ki, float kd, float f) {
      this->r = aim;
      this->kp = kp;
      this->ki = ki;
      this->kd = kd;
      this->freq = f;
      this->t = 1 / f;
    }

    float getAim() { return r; }
    float getKp() { return kp; }
    float getKi() { return ki; }
    float getKd() { return kd; }
    void setAim(float aim) { r = aim; }
    void setKp(float k) { kp = k; }
    void setKi(float k) { ki = k; }
    void setKd(float k) { kd = k; }

    float getError() { return e; }
    float getPreviousError() { return e_pre; }
    float getDiffError() { return de; }
    float getIntegralError() { return ie; }

    float cal(float y) {
      e_pre = e;

      e = r - y;
      ie += (e + e_pre) * t / 2.0;
      de = (e - e_pre) / t;
      u = kp * e + ki * ie + kd * de;
      return u;
    }

};

あとはToioを走らせて係数調整するだけ。