mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-26 01:51:41 +00:00
Move CameraServer and WPILib headers into their own folder
The old headers were moved into folders because doing so avoids polluting the system include directories. Folder names were also normalized to lowercase.
This commit is contained in:
Tyler Veness
committed by
Peter Johnson
Peter Johnson
parent
31ced30c1e
commit
d89b7dd412
404
wpilibc/src/main/native/include/frc/PIDBase.h
Normal file
404
wpilibc/src/main/native/include/frc/PIDBase.h
Normal file
@@ -0,0 +1,404 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <wpi/deprecated.h>
|
||||
#include <wpi/mutex.h>
|
||||
|
||||
#include "frc/Base.h"
|
||||
#include "frc/PIDInterface.h"
|
||||
#include "frc/PIDOutput.h"
|
||||
#include "frc/PIDSource.h"
|
||||
#include "frc/Timer.h"
|
||||
#include "frc/filters/LinearDigitalFilter.h"
|
||||
#include "frc/smartdashboard/SendableBase.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Class implements a PID Control Loop.
|
||||
*
|
||||
* Creates a separate thread which reads the given PIDSource and takes care of
|
||||
* the integral calculations, as well as writing the given PIDOutput.
|
||||
*
|
||||
* This feedback controller runs in discrete time, so time deltas are not used
|
||||
* in the integral and derivative calculations. Therefore, the sample rate
|
||||
* affects the controller's behavior for a given set of PID constants.
|
||||
*/
|
||||
class PIDBase : public SendableBase, public PIDInterface, public PIDOutput {
|
||||
public:
|
||||
/**
|
||||
* Allocate a PID object with the given constants for P, I, D.
|
||||
*
|
||||
* @param Kp the proportional coefficient
|
||||
* @param Ki the integral coefficient
|
||||
* @param Kd the derivative coefficient
|
||||
* @param source The PIDSource object that is used to get values
|
||||
* @param output The PIDOutput object that is set to the output value
|
||||
*/
|
||||
PIDBase(double p, double i, double d, PIDSource& source, PIDOutput& output);
|
||||
|
||||
/**
|
||||
* Allocate a PID object with the given constants for P, I, D.
|
||||
*
|
||||
* @param Kp the proportional coefficient
|
||||
* @param Ki the integral coefficient
|
||||
* @param Kd the derivative coefficient
|
||||
* @param source The PIDSource object that is used to get values
|
||||
* @param output The PIDOutput object that is set to the output value
|
||||
*/
|
||||
PIDBase(double p, double i, double d, double f, PIDSource& source,
|
||||
PIDOutput& output);
|
||||
|
||||
~PIDBase() override = default;
|
||||
|
||||
PIDBase(const PIDBase&) = delete;
|
||||
PIDBase& operator=(const PIDBase) = delete;
|
||||
|
||||
/**
|
||||
* Return the current PID result.
|
||||
*
|
||||
* This is always centered on zero and constrained the the max and min outs.
|
||||
*
|
||||
* @return the latest calculated output
|
||||
*/
|
||||
virtual double Get() const;
|
||||
|
||||
/**
|
||||
* Set the PID controller to consider the input to be continuous,
|
||||
*
|
||||
* Rather then using the max and min input range as constraints, it considers
|
||||
* them to be the same point and automatically calculates the shortest route
|
||||
* to the setpoint.
|
||||
*
|
||||
* @param continuous true turns on continuous, false turns off continuous
|
||||
*/
|
||||
virtual void SetContinuous(bool continuous = true);
|
||||
|
||||
/**
|
||||
* Sets the maximum and minimum values expected from the input.
|
||||
*
|
||||
* @param minimumInput the minimum value expected from the input
|
||||
* @param maximumInput the maximum value expected from the output
|
||||
*/
|
||||
virtual void SetInputRange(double minimumInput, double maximumInput);
|
||||
|
||||
/**
|
||||
* Sets the minimum and maximum values to write.
|
||||
*
|
||||
* @param minimumOutput the minimum value to write to the output
|
||||
* @param maximumOutput the maximum value to write to the output
|
||||
*/
|
||||
virtual void SetOutputRange(double minimumOutput, double maximumOutput);
|
||||
|
||||
/**
|
||||
* Set the PID Controller gain parameters.
|
||||
*
|
||||
* Set the proportional, integral, and differential coefficients.
|
||||
*
|
||||
* @param p Proportional coefficient
|
||||
* @param i Integral coefficient
|
||||
* @param d Differential coefficient
|
||||
*/
|
||||
void SetPID(double p, double i, double d) override;
|
||||
|
||||
/**
|
||||
* Set the PID Controller gain parameters.
|
||||
*
|
||||
* Set the proportional, integral, and differential coefficients.
|
||||
*
|
||||
* @param p Proportional coefficient
|
||||
* @param i Integral coefficient
|
||||
* @param d Differential coefficient
|
||||
* @param f Feed forward coefficient
|
||||
*/
|
||||
virtual void SetPID(double p, double i, double d, double f);
|
||||
|
||||
/**
|
||||
* Set the Proportional coefficient of the PID controller gain.
|
||||
*
|
||||
* @param p proportional coefficient
|
||||
*/
|
||||
void SetP(double p);
|
||||
|
||||
/**
|
||||
* Set the Integral coefficient of the PID controller gain.
|
||||
*
|
||||
* @param i integral coefficient
|
||||
*/
|
||||
void SetI(double i);
|
||||
|
||||
/**
|
||||
* Set the Differential coefficient of the PID controller gain.
|
||||
*
|
||||
* @param d differential coefficient
|
||||
*/
|
||||
void SetD(double d);
|
||||
|
||||
/**
|
||||
* Get the Feed forward coefficient of the PID controller gain.
|
||||
*
|
||||
* @param f Feed forward coefficient
|
||||
*/
|
||||
void SetF(double f);
|
||||
|
||||
/**
|
||||
* Get the Proportional coefficient.
|
||||
*
|
||||
* @return proportional coefficient
|
||||
*/
|
||||
double GetP() const override;
|
||||
|
||||
/**
|
||||
* Get the Integral coefficient.
|
||||
*
|
||||
* @return integral coefficient
|
||||
*/
|
||||
double GetI() const override;
|
||||
|
||||
/**
|
||||
* Get the Differential coefficient.
|
||||
*
|
||||
* @return differential coefficient
|
||||
*/
|
||||
double GetD() const override;
|
||||
|
||||
/**
|
||||
* Get the Feed forward coefficient.
|
||||
*
|
||||
* @return Feed forward coefficient
|
||||
*/
|
||||
virtual double GetF() const;
|
||||
|
||||
/**
|
||||
* Set the setpoint for the PIDBase.
|
||||
*
|
||||
* @param setpoint the desired setpoint
|
||||
*/
|
||||
void SetSetpoint(double setpoint) override;
|
||||
|
||||
/**
|
||||
* Returns the current setpoint of the PIDBase.
|
||||
*
|
||||
* @return the current setpoint
|
||||
*/
|
||||
double GetSetpoint() const override;
|
||||
|
||||
/**
|
||||
* Returns the change in setpoint over time of the PIDBase.
|
||||
*
|
||||
* @return the change in setpoint over time
|
||||
*/
|
||||
double GetDeltaSetpoint() const;
|
||||
|
||||
/**
|
||||
* Returns the current difference of the input from the setpoint.
|
||||
*
|
||||
* @return the current error
|
||||
*/
|
||||
virtual double GetError() const;
|
||||
|
||||
/**
|
||||
* Returns the current average of the error over the past few iterations.
|
||||
*
|
||||
* You can specify the number of iterations to average with
|
||||
* SetToleranceBuffer() (defaults to 1). This is the same value that is used
|
||||
* for OnTarget().
|
||||
*
|
||||
* @return the average error
|
||||
*/
|
||||
WPI_DEPRECATED("Use a LinearDigitalFilter as the input and GetError().")
|
||||
virtual double GetAvgError() const;
|
||||
|
||||
/**
|
||||
* Sets what type of input the PID controller will use.
|
||||
*/
|
||||
virtual void SetPIDSourceType(PIDSourceType pidSource);
|
||||
|
||||
/**
|
||||
* Returns the type of input the PID controller is using.
|
||||
*
|
||||
* @return the PID controller input type
|
||||
*/
|
||||
virtual PIDSourceType GetPIDSourceType() const;
|
||||
|
||||
/**
|
||||
* Set the percentage error which is considered tolerable for use with
|
||||
* OnTarget.
|
||||
*
|
||||
* @param percentage error which is tolerable
|
||||
*/
|
||||
WPI_DEPRECATED("Use SetPercentTolerance() instead.")
|
||||
virtual void SetTolerance(double percent);
|
||||
|
||||
/**
|
||||
* Set the absolute error which is considered tolerable for use with
|
||||
* OnTarget.
|
||||
*
|
||||
* @param percentage error which is tolerable
|
||||
*/
|
||||
virtual void SetAbsoluteTolerance(double absValue);
|
||||
|
||||
/**
|
||||
* Set the percentage error which is considered tolerable for use with
|
||||
* OnTarget.
|
||||
*
|
||||
* @param percentage error which is tolerable
|
||||
*/
|
||||
virtual void SetPercentTolerance(double percentValue);
|
||||
|
||||
/**
|
||||
* Set the number of previous error samples to average for tolerancing. When
|
||||
* determining whether a mechanism is on target, the user may want to use a
|
||||
* rolling average of previous measurements instead of a precise position or
|
||||
* velocity. This is useful for noisy sensors which return a few erroneous
|
||||
* measurements when the mechanism is on target. However, the mechanism will
|
||||
* not register as on target for at least the specified bufLength cycles.
|
||||
*
|
||||
* @param bufLength Number of previous cycles to average. Defaults to 1.
|
||||
*/
|
||||
WPI_DEPRECATED("Use a LinearDigitalFilter as the input.")
|
||||
virtual void SetToleranceBuffer(int buf = 1);
|
||||
|
||||
/**
|
||||
* Return true if the error is within the percentage of the total input range,
|
||||
* determined by SetTolerance. This asssumes that the maximum and minimum
|
||||
* input were set using SetInput.
|
||||
*
|
||||
* Currently this just reports on target as the actual value passes through
|
||||
* the setpoint. Ideally it should be based on being within the tolerance for
|
||||
* some period of time.
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*/
|
||||
virtual bool OnTarget() const;
|
||||
|
||||
/**
|
||||
* Reset the previous error, the integral term, and disable the controller.
|
||||
*/
|
||||
void Reset() override;
|
||||
|
||||
/**
|
||||
* Passes the output directly to SetSetpoint().
|
||||
*
|
||||
* PIDControllers can be nested by passing a PIDController as another
|
||||
* PIDController's output. In that case, the output of the parent controller
|
||||
* becomes the input (i.e., the reference) of the child.
|
||||
*
|
||||
* It is the caller's responsibility to put the data into a valid form for
|
||||
* SetSetpoint().
|
||||
*/
|
||||
void PIDWrite(double output) override;
|
||||
|
||||
void InitSendable(SendableBuilder& builder) override;
|
||||
|
||||
protected:
|
||||
// Is the pid controller enabled
|
||||
bool m_enabled = false;
|
||||
|
||||
mutable wpi::mutex m_thisMutex;
|
||||
|
||||
// Ensures when Disable() is called, PIDWrite() won't run if Calculate()
|
||||
// is already running at that time.
|
||||
mutable wpi::mutex m_pidWriteMutex;
|
||||
|
||||
PIDSource* m_pidInput;
|
||||
PIDOutput* m_pidOutput;
|
||||
Timer m_setpointTimer;
|
||||
|
||||
/**
|
||||
* Read the input, calculate the output accordingly, and write to the output.
|
||||
* This should only be called by the Notifier.
|
||||
*/
|
||||
virtual void Calculate();
|
||||
|
||||
/**
|
||||
* Calculate the feed forward term.
|
||||
*
|
||||
* Both of the provided feed forward calculations are velocity feed forwards.
|
||||
* If a different feed forward calculation is desired, the user can override
|
||||
* this function and provide his or her own. This function does no
|
||||
* synchronization because the PIDBase class only calls it in synchronized
|
||||
* code, so be careful if calling it oneself.
|
||||
*
|
||||
* If a velocity PID controller is being used, the F term should be set to 1
|
||||
* over the maximum setpoint for the output. If a position PID controller is
|
||||
* being used, the F term should be set to 1 over the maximum speed for the
|
||||
* output measured in setpoint units per this controller's update period (see
|
||||
* the default period in this class's constructor).
|
||||
*/
|
||||
virtual double CalculateFeedForward();
|
||||
|
||||
/**
|
||||
* Wraps error around for continuous inputs. The original error is returned if
|
||||
* continuous mode is disabled. This is an unsynchronized function.
|
||||
*
|
||||
* @param error The current error of the PID controller.
|
||||
* @return Error for continuous inputs.
|
||||
*/
|
||||
double GetContinuousError(double error) const;
|
||||
|
||||
private:
|
||||
// Factor for "proportional" control
|
||||
double m_P;
|
||||
|
||||
// Factor for "integral" control
|
||||
double m_I;
|
||||
|
||||
// Factor for "derivative" control
|
||||
double m_D;
|
||||
|
||||
// Factor for "feed forward" control
|
||||
double m_F;
|
||||
|
||||
// |maximum output|
|
||||
double m_maximumOutput = 1.0;
|
||||
|
||||
// |minimum output|
|
||||
double m_minimumOutput = -1.0;
|
||||
|
||||
// Maximum input - limit setpoint to this
|
||||
double m_maximumInput = 0;
|
||||
|
||||
// Minimum input - limit setpoint to this
|
||||
double m_minimumInput = 0;
|
||||
|
||||
// input range - difference between maximum and minimum
|
||||
double m_inputRange = 0;
|
||||
|
||||
// Do the endpoints wrap around? eg. Absolute encoder
|
||||
bool m_continuous = false;
|
||||
|
||||
// The prior error (used to compute velocity)
|
||||
double m_prevError = 0;
|
||||
|
||||
// The sum of the errors for use in the integral calc
|
||||
double m_totalError = 0;
|
||||
|
||||
enum {
|
||||
kAbsoluteTolerance,
|
||||
kPercentTolerance,
|
||||
kNoTolerance
|
||||
} m_toleranceType = kNoTolerance;
|
||||
|
||||
// The percetage or absolute error that is considered on target.
|
||||
double m_tolerance = 0.05;
|
||||
|
||||
double m_setpoint = 0;
|
||||
double m_prevSetpoint = 0;
|
||||
double m_error = 0;
|
||||
double m_result = 0;
|
||||
|
||||
std::shared_ptr<PIDSource> m_origSource;
|
||||
LinearDigitalFilter m_filter{nullptr, {}, {}};
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
Reference in New Issue
Block a user