TurnControl.cpp

[詳解]

#include "TurnControl.h"

TurnControl::TurnControl(int targetBrightness, double Kp, double Ki, double Kd)
  : filter(), pid(static_cast<double>(targetBrightness), Kp, Ki, Kd)
{
}

double TurnControl::calculateTurn(int forward, int currentBrightness, int targetBrightness,
                                  double Kp, double Ki, double Kd)
{
  double filteredBrightness = filter.lowPassFilter(currentBrightness);

  // pidの値を更新(パラメータに変更がなくても更新する)
  pid.setParameter(static_cast<double>(targetBrightness), Kp, Ki, Kd);

  auto pidValue = pid.control(filteredBrightness);

  double forwardPercent = forward / 100.0;
  return pidValue * forwardPercent;
}