WPILibC++  unspecified
PIDController.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2008-2017 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 <atomic>
11 #include <memory>
12 #include <mutex>
13 #include <queue>
14 #include <string>
15 
16 #include "Base.h"
17 #include "Controller.h"
18 #include "LiveWindow/LiveWindow.h"
19 #include "Notifier.h"
20 #include "PIDInterface.h"
21 #include "PIDSource.h"
22 #include "Timer.h"
23 #include "networktables/NetworkTableEntry.h"
24 
25 namespace frc {
26 
27 class PIDOutput;
28 
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  virtual ~PIDController();
46 
47  PIDController(const PIDController&) = delete;
48  PIDController& operator=(const PIDController) = delete;
49 
50  virtual double Get() const;
51  virtual void SetContinuous(bool continuous = true);
52  virtual void SetInputRange(double minimumInput, double maximumInput);
53  virtual void SetOutputRange(double minimumOutput, double maximumOutput);
54  void SetPID(double p, double i, double d) override;
55  virtual void SetPID(double p, double i, double d, double f);
56  double GetP() const override;
57  double GetI() const override;
58  double GetD() const override;
59  virtual double GetF() const;
60 
61  void SetSetpoint(double setpoint) override;
62  double GetSetpoint() const override;
63  double GetDeltaSetpoint() const;
64 
65  virtual double GetError() const;
66  virtual double GetAvgError() const;
67 
68  virtual void SetPIDSourceType(PIDSourceType pidSource);
69  virtual PIDSourceType GetPIDSourceType() const;
70 
71  virtual void SetTolerance(double percent);
72  virtual void SetAbsoluteTolerance(double absValue);
73  virtual void SetPercentTolerance(double percentValue);
74  virtual void SetToleranceBuffer(int buf = 1);
75  virtual bool OnTarget() const;
76 
77  void Enable() override;
78  void Disable() override;
79  bool IsEnabled() const override;
80 
81  void Reset() override;
82 
83  void InitTable(std::shared_ptr<nt::NetworkTable> subtable) override;
84 
85  protected:
86  PIDSource* m_pidInput;
87  PIDOutput* m_pidOutput;
88 
89  nt::NetworkTableEntry m_pEntry;
90  nt::NetworkTableEntry m_iEntry;
91  nt::NetworkTableEntry m_dEntry;
92  nt::NetworkTableEntry m_fEntry;
93  nt::NetworkTableEntry m_setpointEntry;
94  nt::NetworkTableEntry m_enabledEntry;
95  NT_EntryListener m_pListener = 0;
96  NT_EntryListener m_iListener = 0;
97  NT_EntryListener m_dListener = 0;
98  NT_EntryListener m_fListener = 0;
99  NT_EntryListener m_setpointListener = 0;
100  NT_EntryListener m_enabledListener = 0;
101 
102  virtual void Calculate();
103  virtual double CalculateFeedForward();
104  double GetContinuousError(double error) const;
105 
106  private:
107  // factor for "proportional" control
108  double m_P;
109  // factor for "integral" control
110  double m_I;
111  // factor for "derivative" control
112  double m_D;
113  // factor for "feed forward" control
114  double m_F;
115  // |maximum output|
116  double m_maximumOutput = 1.0;
117  // |minimum output|
118  double m_minimumOutput = -1.0;
119  // maximum input - limit setpoint to this
120  double m_maximumInput = 0;
121  // minimum input - limit setpoint to this
122  double m_minimumInput = 0;
123  // do the endpoints wrap around? eg. Absolute encoder
124  bool m_continuous = false;
125  // is the pid controller enabled
126  bool m_enabled = false;
127  // the prior error (used to compute velocity)
128  double m_prevError = 0;
129  // the sum of the errors for use in the integral calc
130  double m_totalError = 0;
131  enum {
132  kAbsoluteTolerance,
133  kPercentTolerance,
134  kNoTolerance
135  } m_toleranceType = kNoTolerance;
136 
137  // the percetage or absolute error that is considered on target.
138  double m_tolerance = 0.05;
139  double m_setpoint = 0;
140  double m_prevSetpoint = 0;
141  double m_error = 0;
142  double m_result = 0;
143  double m_period;
144 
145  // Length of buffer for averaging for tolerances.
146  std::atomic<unsigned> m_bufLength{1};
147  std::queue<double> m_buf;
148  double m_bufTotal = 0;
149 
150  mutable std::mutex m_mutex;
151 
152  std::unique_ptr<Notifier> m_controlLoop;
153  Timer m_setpointTimer;
154 
155  std::string GetSmartDashboardType() const override;
156  void UpdateTable() override;
157  void StartLiveWindowMode() override;
158  void StopLiveWindowMode() override;
159  void RemoveListeners();
160 };
161 
162 } // namespace frc
virtual void SetInputRange(double minimumInput, double maximumInput)
Sets the maximum and minimum values expected from the input.
Definition: PIDController.cpp:311
virtual double Get() const
Return the current PID result.
Definition: PIDController.cpp:286
Definition: Timer.cpp:18
double GetD() const override
Get the Differential coefficient.
Definition: PIDController.cpp:264
virtual void SetPIDSourceType(PIDSourceType pidSource)
Sets what type of input the PID controller will use.
Definition: PIDController.cpp:399
Live Window Sendable is a special type of object sendable to the live window.
Definition: LiveWindowSendable.h:17
virtual double GetAvgError() const
Returns the current average of the error over the past few iterations.
Definition: PIDController.cpp:419
virtual void SetContinuous(bool continuous=true)
Set the PID controller to consider the input to be continuous,.
Definition: PIDController.cpp:300
virtual PIDSourceType GetPIDSourceType() const
Returns the type of input the PID controller is using.
Definition: PIDController.cpp:407
PIDSource interface is a generic sensor source for the PID class.
Definition: PIDSource.h:19
Class implements a PID Control Loop.
Definition: PIDController.h:39
virtual double CalculateFeedForward()
Calculate the feed forward term.
Definition: PIDController.cpp:181
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:378
virtual double GetF() const
Get the Feed forward coefficient.
Definition: PIDController.cpp:274
Timer objects measure accumulated time in seconds.
Definition: Timer.h:30
double GetContinuousError(double error) const
Wraps error around for continuous inputs.
Definition: PIDController.cpp:645
double GetP() const override
Get the Proportional coefficient.
Definition: PIDController.cpp:244
virtual double GetError() const
Returns the current difference of the input from the setpoint.
Definition: PIDController.cpp:388
void Reset() override
Reset the previous error, the integral term, and disable the controller.
Definition: PIDController.cpp:557
bool IsEnabled() const override
Return true if PIDController is enabled.
Definition: PIDController.cpp:549
Definition: PIDInterface.h:16
void SetPID(double p, double i, double d) override
Set the PID Controller gain parameters.
Definition: PIDController.cpp:201
double GetSetpoint() const override
Returns the current setpoint of the PIDController.
Definition: PIDController.cpp:368
virtual void SetOutputRange(double minimumOutput, double maximumOutput)
Sets the minimum and maximum values to write.
Definition: PIDController.cpp:327
double GetI() const override
Get the Integral coefficient.
Definition: PIDController.cpp:254
NetworkTables Entry.
Definition: NetworkTableEntry.h:30
void InitTable(std::shared_ptr< nt::NetworkTable > subtable) override
Initializes a table for this sendable object.
Definition: PIDController.cpp:570
void SetSetpoint(double setpoint) override
Set the setpoint for the PIDController.
Definition: PIDController.cpp:340
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:88
void Disable() override
Stop running the PIDController, this sets the output to zero before stopping.
Definition: PIDController.cpp:536
void Enable() override
Begin running the PIDController.
Definition: PIDController.cpp:524