WPILibC++  unspecified
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Pages
PIDController.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) FIRST 2008-2016. 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 "Base.h"
11 #include "Controller.h"
12 #include "LiveWindow/LiveWindow.h"
13 #include "PIDInterface.h"
14 #include "PIDSource.h"
15 #include "Notifier.h"
16 #include "HAL/cpp/priority_mutex.h"
17 #include "Timer.h"
18 
19 #include <memory>
20 
21 #include <atomic>
22 #include <queue>
23 
24 class PIDOutput;
25 
34  public PIDInterface,
35  public ITableListener {
36  public:
37  PIDController(float p, float i, float d, PIDSource *source, PIDOutput *output,
38  float period = 0.05);
39  PIDController(float p, float i, float d, float f, PIDSource *source,
40  PIDOutput *output, float period = 0.05);
41  virtual ~PIDController();
42 
43  PIDController(const PIDController&) = delete;
44  PIDController& operator=(const PIDController) = delete;
45 
46  virtual float Get() const;
47  virtual void SetContinuous(bool continuous = true);
48  virtual void SetInputRange(float minimumInput, float maximumInput);
49  virtual void SetOutputRange(float minimumOutput, float maximumOutput);
50  virtual void SetPID(double p, double i, double d) override;
51  virtual void SetPID(double p, double i, double d, double f);
52  virtual double GetP() const override;
53  virtual double GetI() const override;
54  virtual double GetD() const override;
55  virtual double GetF() const;
56 
57  virtual void SetSetpoint(float setpoint) override;
58  virtual double GetSetpoint() const override;
59  double GetDeltaSetpoint() const;
60 
61  virtual float GetError() const;
62  virtual float GetAvgError() const;
63 
64  virtual void SetPIDSourceType(PIDSourceType pidSource);
65  virtual PIDSourceType GetPIDSourceType() const;
66 
67  virtual void SetTolerance(float percent);
68  virtual void SetAbsoluteTolerance(float absValue);
69  virtual void SetPercentTolerance(float percentValue);
70  virtual void SetToleranceBuffer(unsigned buf = 1);
71  virtual bool OnTarget() const;
72 
73  virtual void Enable() override;
74  virtual void Disable() override;
75  virtual bool IsEnabled() const override;
76 
77  virtual void Reset() override;
78 
79  virtual void InitTable(std::shared_ptr<ITable> table) override;
80 
81  protected:
82  PIDSource *m_pidInput;
83  PIDOutput *m_pidOutput;
84 
85  std::shared_ptr<ITable> m_table;
86  virtual void Calculate();
87  virtual double CalculateFeedForward();
88 
89  private:
90  float m_P; // factor for "proportional" control
91  float m_I; // factor for "integral" control
92  float m_D; // factor for "derivative" control
93  float m_F; // factor for "feed forward" control
94  float m_maximumOutput = 1.0; // |maximum output|
95  float m_minimumOutput = -1.0; // |minimum output|
96  float m_maximumInput = 0; // maximum input - limit setpoint to this
97  float m_minimumInput = 0; // minimum input - limit setpoint to this
98  bool m_continuous = false; // do the endpoints wrap around? eg. Absolute encoder
99  bool m_enabled = false; // is the pid controller enabled
100  float m_prevError = 0; // the prior error (used to compute velocity)
101  double m_totalError = 0; // the sum of the errors for use in the integral calc
102  enum {
103  kAbsoluteTolerance,
104  kPercentTolerance,
105  kNoTolerance
106  } m_toleranceType = kNoTolerance;
107 
108  // the percetage or absolute error that is considered on target.
109  float m_tolerance = 0.05;
110  float m_setpoint = 0;
111  float m_prevSetpoint = 0;
112  float m_error = 0;
113  float m_result = 0;
114  float m_period;
115 
116  // Length of buffer for averaging for tolerances.
117  std::atomic<unsigned> m_bufLength{1};
118  std::queue<double> m_buf;
119  double m_bufTotal = 0;
120 
121  mutable priority_recursive_mutex m_mutex;
122 
123  std::unique_ptr<Notifier> m_controlLoop;
124  Timer m_setpointTimer;
125 
126  void Initialize(float p, float i, float d, float f, PIDSource *source,
127  PIDOutput *output, float period = 0.05);
128 
129  virtual std::shared_ptr<ITable> GetTable() const override;
130  virtual std::string GetSmartDashboardType() const override;
131  virtual void ValueChanged(ITable *source, llvm::StringRef key,
132  std::shared_ptr<nt::Value> value,
133  bool isNew) override;
134  virtual void UpdateTable() override;
135  virtual void StartLiveWindowMode() override;
136  virtual void StopLiveWindowMode() override;
137 };
virtual void InitTable(std::shared_ptr< ITable > table) override
Initializes a table for this sendable object.
Definition: PIDController.cpp:563
virtual void Disable() override
Stop running the PIDController, this sets the output to zero before stopping.
Definition: PIDController.cpp:527
A table whose values can be read and written to.
Definition: ITable.h:43
virtual void SetOutputRange(float minimumOutput, float maximumOutput)
Sets the minimum and maximum values to write.
Definition: PIDController.cpp:325
Class implements a PID Control Loop.
Definition: PIDController.h:33
double GetDeltaSetpoint() const
Returns the change in setpoint over time of the PIDController.
Definition: PIDController.cpp:375
Live Window Sendable is a special type of object sendable to the live window.
Definition: LiveWindowSendable.h:18
virtual void SetSetpoint(float setpoint) override
Set the setpoint for the PIDController Clears the queue for GetAvgError().
Definition: PIDController.cpp:338
virtual void SetContinuous(bool continuous=true)
Set the PID controller to consider the input to be continuous, Rather then using the max and min in a...
Definition: PIDController.cpp:298
virtual double CalculateFeedForward()
Calculate the feed forward term.
Definition: PIDController.cpp:186
virtual float GetAvgError() const
Returns the current average of the error over the past few iterations.
Definition: PIDController.cpp:413
virtual double GetSetpoint() const override
Returns the current setpoint of the PIDController.
Definition: PIDController.cpp:366
PIDOutput interface is a generic output for the PID class.
Definition: PIDOutput.h:18
virtual double GetD() const override
Get the Differential coefficient.
Definition: PIDController.cpp:267
Definition: priority_mutex.h:22
virtual void Calculate()
Read the input, calculate the output accordingly, and write to the output.
Definition: PIDController.cpp:85
virtual double GetF() const
Get the Feed forward coefficient.
Definition: PIDController.cpp:276
virtual void Enable() override
Begin running the PIDController.
Definition: PIDController.cpp:513
virtual void SetInputRange(float minimumInput, float maximumInput)
Sets the maximum and minimum values expected from the input.
Definition: PIDController.cpp:309
virtual double GetP() const override
Get the Proportional coefficient.
Definition: PIDController.cpp:249
A listener that listens to changes in values in a ITable.
Definition: ITableListener.h:18
PIDSource interface is a generic sensor source for the PID class.
Definition: PIDSource.h:18
Definition: PIDInterface.h:14
virtual float GetError() const
Returns the current difference of the input from the setpoint.
Definition: PIDController.cpp:384
virtual bool IsEnabled() const override
Return true if PIDController is enabled.
Definition: PIDController.cpp:542
PIDController(float p, float i, float d, PIDSource *source, PIDOutput *output, float period=0.05)
Allocate a PID object with the given constants for P, I, D.
Definition: PIDController.cpp:34
virtual void SetPID(double p, double i, double d) override
Set the PID Controller gain parameters.
Definition: PIDController.cpp:205
Timer objects measure accumulated time in seconds.
Definition: Timer.h:29
virtual float Get() const
Return the current PID result This is always centered on zero and constrained the the max and min out...
Definition: PIDController.cpp:286
virtual void SetPIDSourceType(PIDSourceType pidSource)
Sets what type of input the PID controller will use.
Definition: PIDController.cpp:396
StringRef - Represent a constant reference to a string, i.e.
Definition: StringRef.h:39
virtual double GetI() const override
Get the Integral coefficient.
Definition: PIDController.cpp:258
virtual PIDSourceType GetPIDSourceType() const
Returns the type of input the PID controller is using.
Definition: PIDController.cpp:403
virtual void Reset() override
Reset the previous error,, the integral term, and disable the controller.
Definition: PIDController.cpp:550