Pid.cpp

[詳解]

#include "Pid.h"

PidGain::PidGain(double Kp_, double Ki_, double Kd_) : Kp(Kp_), Ki(Ki_), Kd(Kd_) {}

Pid::Pid(double target_, double Kp_, double Ki_, double Kd_)
  : target(target_), gain(Kp_, Ki_, Kd_), integral(0.0f), preError(0.0f)
{
}

void PidGain::setPidGain(double Kp_, double Ki_, double Kd_)
{
  Kp = Kp_;
  Ki = Ki_;
  Kd = Kd_;
}

double Pid::control(double value, double delta)
{
  // 目標値と現在値との偏差を求める
  double error = target - value;
  // 偏差の積分処理
  integral += error * delta;
  // 偏差の微分処理
  double diff = (error - preError) / delta;

  // 前回偏差の更新
  preError = error;

  // P制御の計算(Pゲイン * 偏差)
  double p = gain.Kp * error;
  // I制御の計算(Iゲイン * 偏差の積分値)
  double i = gain.Ki * integral;
  // D制御の計算(Dゲイン * 偏差の微分値)
  double d = gain.Kd * diff;

  return limit(p + i + d);
}

const double Pid::setParameter(double target_, double Kp_, double Ki_, double Kd_)
{
  target = target_;
  setPidGain(Kp_, Ki_, Kd_);

  return target;
}

const PidGain& Pid::setPidGain(double Kp_, double Ki_, double Kd_)
{
  gain.setPidGain(Kp_, Ki_, Kd_);
  return gain;
}

double Pid::limit(double value)
{
  if(value > 100.0) return 100.0;
  if(value < -100.0) return -100.0;

  return value;
}