iterativePosPidController.hpp

Go to the documentation of this file.

/*
 * Based on the Arduino PID controller: https://github.com/br3ttb/Arduino-PID-Library
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
#pragma once
 
#include "okapi/api/control/iterative/iterativePositionController.hpp"
#include "okapi/api/control/util/settledUtil.hpp"
#include "okapi/api/filter/filter.hpp"
#include "okapi/api/filter/passthroughFilter.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <limits>
#include <memory>
 
namespace okapi {
class IterativePosPIDController : public IterativePositionController<double, double> {
  public:
  struct Gains {
    double kP{0};
    double kI{0};
    double kD{0};
    double kBias{0};
 
    bool operator==(const Gains &rhs) const;
    bool operator!=(const Gains &rhs) const;
  };
 
  IterativePosPIDController(
    double ikP,
    double ikI,
    double ikD,
    double ikBias,
    const TimeUtil &itimeUtil,
    std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
 
  IterativePosPIDController(
    const Gains &igains,
    const TimeUtil &itimeUtil,
    std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
 
  double step(double inewReading) override;
 
  void setTarget(double itarget) override;
 
  void controllerSet(double ivalue) override;
 
  double getTarget() override;
 
  double getTarget() const;
 
  double getProcessValue() const override;
 
  double getOutput() const override;
 
  double getMaxOutput() override;
 
  double getMinOutput() override;
 
  double getError() const override;
 
  bool isSettled() override;
 
  void setSampleTime(QTime isampleTime) override;
 
  void setOutputLimits(double imax, double imin) override;
 
  void setControllerSetTargetLimits(double itargetMax, double itargetMin) override;
 
  void reset() override;
 
  void flipDisable() override;
 
  void flipDisable(bool iisDisabled) override;
 
  bool isDisabled() const override;
 
  QTime getSampleTime() const override;
 
  virtual void setIntegralLimits(double imax, double imin);
 
  virtual void setErrorSumLimits(double imax, double imin);
 
  virtual void setIntegratorReset(bool iresetOnZero);
 
  virtual void setGains(const Gains &igains);
 
  Gains getGains() const;
 
  protected:
  std::shared_ptr<Logger> logger;
  double kP, kI, kD, kBias;
  QTime sampleTime{10_ms};
  double target{0};
  double lastReading{0};
  double error{0};
  double lastError{0};
  std::unique_ptr<Filter> derivativeFilter;
 
  // Integral bounds
  double integral{0};
  double integralMax{1};
  double integralMin{-1};
 
  // Error will only be added to the integral term within these bounds on either side of the target
  double errorSumMin{0};
  double errorSumMax{std::numeric_limits<double>::max()};
 
  double derivative{0};
 
  // Output bounds
  double output{0};
  double outputMax{1};
  double outputMin{-1};
  double controllerSetTargetMax{1};
  double controllerSetTargetMin{-1};
 
  // Reset the integrated when the controller crosses 0 or not
  bool shouldResetOnCross{true};
 
  bool controllerIsDisabled{false};
 
  std::unique_ptr<AbstractTimer> loopDtTimer;
  std::unique_ptr<SettledUtil> settledUtil;
};
} // namespace okapi