アンチワインドアップなPID制御のライブラリを作った

書いたライブラリ

PID.h

#ifndef PID_H
#define PID_H
#include <mbed.h>

class PID {
  private:
    /* data */
    float p, i, d; // PID gain
    float dt;      // interval of PID loop [s]
    Ticker ticker;
    RawSerial *pc;
    float lastError;
    float error;

    float output, _output;
    float limit;
    float min, max;

    float pTerm;
    float iTerm, _iTerm;
    float dTerm;

    void compute();

  public:
    PID(float _p, float _i, float _d, float _dt, RawSerial *_pc);
    void setLimit(float limit);
    void setGain(float _p, float _i, float _d);
    void appendError(float _error);
    float getOutput();
};

#endif

PID.cpp

#include "PID.h"

PID::PID(float _p, float _i, float _d, float _dt, RawSerial *_pc) : ticker() {
    pc = _pc;
    p = _p;
    i = _i;
    d = _d;
    dt = _dt;
    limit = 1000;
    setLimit(limit);
    lastError = 0;
    pc->printf("PID init\r");
    ticker.attach(callback(this, &PID::compute), _dt);
}

void PID::setLimit(float _limit) {
    limit = abs(_limit);
    min = -limit;
    max = limit;
}
void PID::setGain(float _p, float _i, float _d) {
    p = _p;
    i = _i;
    d = _d;
}

void PID::appendError(float _error) {
    error = _error;
}

void PID::compute() {
    // please append error before compute
    _iTerm = (abs(_output) > limit ? 0 : i * error * dt); // アンチワインドアップ
    pTerm = p * error;                                    // 比例
    iTerm += _iTerm;                                      // 積分
    dTerm = d * (error - lastError) / dt;                 // 微分
    lastError = error;

    _output = (pTerm + iTerm + dTerm);

    if (_output > max) {
        output = max;
    } else if (_output < min) {
        output = min;
    } else {
        output = _output;
    }
    pc->printf("p:%f i:%f d:%f Output: %f _out:%f\n", pTerm, iTerm, dTerm, output, _output);
}

float PID::getOutput() {
    return output;
}

アンチワインドアップPIDで姿勢制御

姿勢制御のコード

#include "mbed.h"
#include "modeRun.h"
float deg;
PID attitudeCtrl(0.7, 1, 0.07, 10e-3, &pc);
Ticker changeTarget;

float targetDeg = 0;

void setup() {
    swDrible.mode(PullUp);
    swKicker.mode(PullUp);
    // dribler.write(0);
    dribbler.turnOff();
    pc.baud(2000000);
    imu.init();
    MD.setVelocityZero();
    initModeRun();
    imu.setZero();
    timer.start();
    pidDt.start();
    tickCalcIMU.attach_us(&attitudeControl, 6000);
}

int main(void) {
    setup();
    attitudeCtrl.setLimit(30);
    while (1) {
        // modeRun();
        deg = imu.getDeg();
        attitudeCtrl.appendError(degBetween_signed(targetDeg, deg));
        int16_t m_power = (int16_t)attitudeCtrl.getOutput();
        MD.setMotors(info, m_power, m_power, m_power, m_power);
    }
}

なにを変えたか

• I成分の発散を抑えるためにアンチワインドアップを入れた

• 更新周期を10msに変更した(センサの更新周期に合わせた)

• (ゲインを変えた)

なんのために書いたか

• これはモータドライバのfirmwareを書くために作りました．

• PIDライブラリの検証をするために姿勢制御を書いただけ．

• 動作したからOK．ついでに姿勢制御も極めてみたよ！！ついでにね！！

感想

• 理論的に正しいとされている手法で書き直すことができて心は満足

• 理論的に正しい方法でそれっぽく制御できてとても満足．