WPILibC++  unspecified
PIDController.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <memory>
11 #include <string>
12 
13 #include <support/deprecated.h>
14 #include <support/mutex.h>
15 
16 #include "Base.h"
17 #include "Controller.h"
18 #include "Filters/LinearDigitalFilter.h"
19 #include "Notifier.h"
20 #include "PIDInterface.h"
21 #include "PIDSource.h"
22 #include "SmartDashboard/SendableBase.h"
23 #include "Timer.h"
24 
25 namespace frc {
26 
27 class PIDOutput;
28 
39 class PIDController : public SendableBase, public PIDInterface {
40  public:
41  PIDController(double p, double i, double d, PIDSource* source,
42  PIDOutput* output, double period = 0.05);
43  PIDController(double p, double i, double d, double f, PIDSource* source,
44  PIDOutput* output, double period = 0.05);
45  PIDController(double p, double i, double d, PIDSource& source,
46  PIDOutput& output, double period = 0.05);
47  PIDController(double p, double i, double d, double f, PIDSource& source,
48  PIDOutput& output, double period = 0.05);
49  ~PIDController() override;
50 
51  PIDController(const PIDController&) = delete;
52  PIDController& operator=(const PIDController) = delete;
53 
54  virtual double Get() const;
55  virtual void SetContinuous(bool continuous = true);
56  virtual void SetInputRange(double minimumInput, double maximumInput);
57  virtual void SetOutputRange(double minimumOutput, double maximumOutput);
58  void SetPID(double p, double i, double d) override;
59  virtual void SetPID(double p, double i, double d, double f);
60  void SetP(double p);
61  void SetI(double i);
62  void SetD(double d);
63  void SetF(double f);
64  double GetP() const override;
65  double GetI() const override;
66  double GetD() const override;
67  virtual double GetF() const;
68 
69  void SetSetpoint(double setpoint) override;
70  double GetSetpoint() const override;
71  double GetDeltaSetpoint() const;
72 
73  virtual double GetError() const;
74 
75  WPI_DEPRECATED("Use a LinearDigitalFilter as the input and GetError().")
76  virtual double GetAvgError() const;
77 
78  virtual void SetPIDSourceType(PIDSourceType pidSource);
79  virtual PIDSourceType GetPIDSourceType() const;
80 
81  WPI_DEPRECATED("Use SetPercentTolerance() instead.")
82  virtual void SetTolerance(double percent);
83  virtual void SetAbsoluteTolerance(double absValue);
84  virtual void SetPercentTolerance(double percentValue);
85 
86  WPI_DEPRECATED("Use a LinearDigitalFilter as the input.")
87  virtual void SetToleranceBuffer(int buf = 1);
88 
89  virtual bool OnTarget() const;
90 
91  void Enable() override;
92  void Disable() override;
93  void SetEnabled(bool enable);
94  bool IsEnabled() const override;
95 
96  void Reset() override;
97 
98  void InitSendable(SendableBuilder& builder) override;
99 
100  protected:
101  PIDSource* m_pidInput;
102  PIDOutput* m_pidOutput;
103 
104  virtual void Calculate();
105  virtual double CalculateFeedForward();
106  double GetContinuousError(double error) const;
107 
108  private:
109  // Factor for "proportional" control
110  double m_P;
111 
112  // Factor for "integral" control
113  double m_I;
114 
115  // Factor for "derivative" control
116  double m_D;
117 
118  // Factor for "feed forward" control
119  double m_F;
120 
121  // |maximum output|
122  double m_maximumOutput = 1.0;
123 
124  // |minimum output|
125  double m_minimumOutput = -1.0;
126 
127  // Maximum input - limit setpoint to this
128  double m_maximumInput = 0;
129 
130  // Minimum input - limit setpoint to this
131  double m_minimumInput = 0;
132 
133  // input range - difference between maximum and minimum
134  double m_inputRange = 0;
135 
136  // Do the endpoints wrap around? eg. Absolute encoder
137  bool m_continuous = false;
138 
139  // Is the pid controller enabled
140  bool m_enabled = false;
141 
142  // The prior error (used to compute velocity)
143  double m_prevError = 0;
144 
145  // The sum of the errors for use in the integral calc
146  double m_totalError = 0;
147 
148  enum {
149  kAbsoluteTolerance,
150  kPercentTolerance,
151  kNoTolerance
152  } m_toleranceType = kNoTolerance;
153 
154  // The percetage or absolute error that is considered on target.
155  double m_tolerance = 0.05;
156 
157  double m_setpoint = 0;
158  double m_prevSetpoint = 0;
159  double m_error = 0;
160  double m_result = 0;
161  double m_period;
162 
163  std::shared_ptr<PIDSource> m_origSource;
164  LinearDigitalFilter m_filter{nullptr, {}, {}};
165 
166  mutable wpi::mutex m_thisMutex;
167 
168  // Ensures when Disable() is called, PIDWrite() won't run if Calculate()
169  // is already running at that time.
170  mutable wpi::mutex m_pidWriteMutex;
171 
172  std::unique_ptr<Notifier> m_controlLoop;
173  Timer m_setpointTimer;
174 };
175 
176 } // namespace frc
virtual void SetInputRange(double minimumInput, double maximumInput)
Sets the maximum and minimum values expected from the input.
Definition: PIDController.cpp:387
virtual double Get() const
Return the current PID result.
Definition: PIDController.cpp:362
Definition: RobotController.cpp:14
double GetD() const override
Get the Differential coefficient.
Definition: PIDController.cpp:340
void SetD(double d)
Set the Differential coefficient of the PID controller gain.
Definition: PIDController.cpp:300
virtual void SetPIDSourceType(PIDSourceType pidSource)
Sets what type of input the PID controller will use.
Definition: PIDController.cpp:478
virtual double GetAvgError() const
Returns the current average of the error over the past few iterations.
Definition: PIDController.cpp:473
virtual void SetContinuous(bool continuous=true)
Set the PID controller to consider the input to be continuous,.
Definition: PIDController.cpp:376
virtual PIDSourceType GetPIDSourceType() const
Returns the type of input the PID controller is using.
Definition: PIDController.cpp:486
PIDSource interface is a generic sensor source for the PID class.
Definition: PIDSource.h:20
Class implements a PID Control Loop.
Definition: PIDController.h:39
void InitSendable(SendableBuilder &builder) override
Initializes this Sendable object.
Definition: PIDController.cpp:630
virtual double CalculateFeedForward()
Calculate the feed forward term.
Definition: PIDController.cpp:228
PIDOutput interface is a generic output for the PID class.
Definition: PIDOutput.h:20
double GetDeltaSetpoint() const
Returns the change in setpoint over time of the PIDController.
Definition: PIDController.cpp:447
virtual double GetF() const
Get the Feed forward coefficient.
Definition: PIDController.cpp:350
Timer objects measure accumulated time in seconds.
Definition: Timer.h:33
double GetContinuousError(double error) const
Wraps error around for continuous inputs.
Definition: PIDController.cpp:654
void SetF(double f)
Get the Feed forward coefficient of the PID controller gain.
Definition: PIDController.cpp:310
double GetP() const override
Get the Proportional coefficient.
Definition: PIDController.cpp:320
virtual double GetError() const
Returns the current difference of the input from the setpoint.
Definition: PIDController.cpp:457
void Reset() override
Reset the previous error, the integral term, and disable the controller.
Definition: PIDController.cpp:621
bool IsEnabled() const override
Return true if PIDController is enabled.
Definition: PIDController.cpp:613
Definition: PIDInterface.h:15
void SetPID(double p, double i, double d) override
Set the PID Controller gain parameters.
Definition: PIDController.cpp:248
double GetSetpoint() const override
Returns the current setpoint of the PIDController.
Definition: PIDController.cpp:437
Definition: SendableBase.h:19
virtual void SetOutputRange(double minimumOutput, double maximumOutput)
Sets the minimum and maximum values to write.
Definition: PIDController.cpp:404
double GetI() const override
Get the Integral coefficient.
Definition: PIDController.cpp:330
Definition: SendableBuilder.h:23
void SetP(double p)
Set the Proportional coefficient of the PID controller gain.
Definition: PIDController.cpp:280
void SetEnabled(bool enable)
Set the enabled state of the PIDController.
Definition: PIDController.cpp:602
This class implements a linear, digital filter.
Definition: LinearDigitalFilter.h:70
void SetSetpoint(double setpoint) override
Set the setpoint for the PIDController.
Definition: PIDController.cpp:415
PIDController(double p, double i, double d, PIDSource *source, PIDOutput *output, double period=0.05)
Allocate a PID object with the given constants for P, I, D.
Definition: PIDController.cpp:40
virtual void Calculate()
Read the input, calculate the output accordingly, and write to the output.
Definition: PIDController.cpp:128
void SetI(double i)
Set the Integral coefficient of the PID controller gain.
Definition: PIDController.cpp:290
void Disable() override
Stop running the PIDController, this sets the output to zero before stopping.
Definition: PIDController.cpp:586
void Enable() override
Begin running the PIDController.
Definition: PIDController.cpp:576