277 lines
7.9 KiB
C++
277 lines
7.9 KiB
C++
/*
|
|
* 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;
|
|
};
|
|
|
|
/**
|
|
* Position PID controller.
|
|
*
|
|
* @param ikP the proportional gain
|
|
* @param ikI the integration gain
|
|
* @param ikD the derivative gain
|
|
* @param ikBias the controller bias
|
|
* @param itimeUtil see TimeUtil docs
|
|
* @param iderivativeFilter a filter for filtering the derivative term
|
|
* @param ilogger The logger this instance will log to.
|
|
*/
|
|
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());
|
|
|
|
/**
|
|
* Position PID controller.
|
|
*
|
|
* @param igains the controller gains
|
|
* @param itimeUtil see TimeUtil docs
|
|
* @param iderivativeFilter a filter for filtering the derivative term
|
|
*/
|
|
IterativePosPIDController(
|
|
const Gains &igains,
|
|
const TimeUtil &itimeUtil,
|
|
std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
|
|
std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
|
|
|
|
/**
|
|
* Do one iteration of the controller. Returns the reading in the range [-1, 1] unless the
|
|
* bounds have been changed with setOutputLimits().
|
|
*
|
|
* @param inewReading new measurement
|
|
* @return controller output
|
|
*/
|
|
double step(double inewReading) override;
|
|
|
|
/**
|
|
* Sets the target for the controller.
|
|
*
|
|
* @param itarget new target position
|
|
*/
|
|
void setTarget(double itarget) override;
|
|
|
|
/**
|
|
* Writes the value of the controller output. This method might be automatically called in another
|
|
* thread by the controller. The range of input values is expected to be [-1, 1].
|
|
*
|
|
* @param ivalue the controller's output in the range [-1, 1]
|
|
*/
|
|
void controllerSet(double ivalue) override;
|
|
|
|
/**
|
|
* Gets the last set target, or the default target if none was set.
|
|
*
|
|
* @return the last target
|
|
*/
|
|
double getTarget() override;
|
|
|
|
/**
|
|
* Gets the last set target, or the default target if none was set.
|
|
*
|
|
* @return the last target
|
|
*/
|
|
double getTarget() const;
|
|
|
|
/**
|
|
* @return The most recent value of the process variable.
|
|
*/
|
|
double getProcessValue() const override;
|
|
|
|
/**
|
|
* Returns the last calculated output of the controller. Output is in the range [-1, 1]
|
|
* unless the bounds have been changed with setOutputLimits().
|
|
*/
|
|
double getOutput() const override;
|
|
|
|
/**
|
|
* Get the upper output bound.
|
|
*
|
|
* @return the upper output bound
|
|
*/
|
|
double getMaxOutput() override;
|
|
|
|
/**
|
|
* Get the lower output bound.
|
|
*
|
|
* @return the lower output bound
|
|
*/
|
|
double getMinOutput() override;
|
|
|
|
/**
|
|
* Returns the last error of the controller. Does not update when disabled.
|
|
*/
|
|
double getError() const override;
|
|
|
|
/**
|
|
* Returns whether the controller has settled at the target. Determining what settling means is
|
|
* implementation-dependent.
|
|
*
|
|
* If the controller is disabled, this method must return true.
|
|
*
|
|
* @return whether the controller is settled
|
|
*/
|
|
bool isSettled() override;
|
|
|
|
/**
|
|
* Set time between loops in ms.
|
|
*
|
|
* @param isampleTime time between loops
|
|
*/
|
|
void setSampleTime(QTime isampleTime) override;
|
|
|
|
/**
|
|
* Set controller output bounds. Default bounds are [-1, 1].
|
|
*
|
|
* @param imax max output
|
|
* @param imin min output
|
|
*/
|
|
void setOutputLimits(double imax, double imin) override;
|
|
|
|
/**
|
|
* Sets the (soft) limits for the target range that controllerSet() scales into. The target
|
|
* computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
|
|
*
|
|
* @param itargetMax The new max target for controllerSet().
|
|
* @param itargetMin The new min target for controllerSet().
|
|
*/
|
|
void setControllerSetTargetLimits(double itargetMax, double itargetMin) override;
|
|
|
|
/**
|
|
* Resets the controller's internal state so it is similar to when it was first initialized, while
|
|
* keeping any user-configured information.
|
|
*/
|
|
void reset() override;
|
|
|
|
/**
|
|
* Changes whether the controller is off or on. Turning the controller on after it was off will
|
|
* cause the controller to move to its last set target, unless it was reset in that time.
|
|
*/
|
|
void flipDisable() override;
|
|
|
|
/**
|
|
* Sets whether the controller is off or on. Turning the controller on after it was off will
|
|
* cause the controller to move to its last set target, unless it was reset in that time.
|
|
*
|
|
* @param iisDisabled whether the controller is disabled
|
|
*/
|
|
void flipDisable(bool iisDisabled) override;
|
|
|
|
/**
|
|
* Returns whether the controller is currently disabled.
|
|
*
|
|
* @return whether the controller is currently disabled
|
|
*/
|
|
bool isDisabled() const override;
|
|
|
|
/**
|
|
* Get the last set sample time.
|
|
*
|
|
* @return sample time
|
|
*/
|
|
QTime getSampleTime() const override;
|
|
|
|
/**
|
|
* Set integrator bounds. Default bounds are [-1, 1].
|
|
*
|
|
* @param imax max integrator value
|
|
* @param imin min integrator value
|
|
*/
|
|
virtual void setIntegralLimits(double imax, double imin);
|
|
|
|
/**
|
|
* Set the error sum bounds. Default bounds are [0, std::numeric_limits<double>::max()]. Error
|
|
* will only be added to the integral term when its absolute value is between these bounds of
|
|
* either side of the target.
|
|
*
|
|
* @param imax max error value that will be summed
|
|
* @param imin min error value that will be summed
|
|
*/
|
|
virtual void setErrorSumLimits(double imax, double imin);
|
|
|
|
/**
|
|
* Set whether the integrator should be reset when error is 0 or changes sign.
|
|
*
|
|
* @param iresetOnZero true to reset
|
|
*/
|
|
virtual void setIntegratorReset(bool iresetOnZero);
|
|
|
|
/**
|
|
* Set controller gains.
|
|
*
|
|
* @param igains The new gains.
|
|
*/
|
|
virtual void setGains(const Gains &igains);
|
|
|
|
/**
|
|
* Gets the current gains.
|
|
*
|
|
* @return The current gains.
|
|
*/
|
|
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
|