Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / f7f267a91e70db6e5f62b09d88f0d02f15c904e8 / . / wpilibc / shared / include / PIDController.h
blob: a304f9ccfaa5da6c88b8bf63d4def00487841d50 [file] [log] [blame]
Brian Silvermanf7f267a2017-02-04 16:16:08 -0800[diff] [blame^]1/*----------------------------------------------------------------------------*/
2/* Copyright (c) FIRST 2008-2017. 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 <queue>
13#include <string>
14
15#include "Base.h"
16#include "Controller.h"
17#include "HAL/cpp/priority_mutex.h"
18#include "LiveWindow/LiveWindow.h"
19#include "Notifier.h"
20#include "PIDInterface.h"
21#include "PIDSource.h"
22#include "Timer.h"
23
24namespace frc {
25
26class PIDOutput;
27
28/**
29 * Class implements a PID Control Loop.
30 *
31 * Creates a separate thread which reads the given PIDSource and takes
32 * care of the integral calculations, as well as writing the given PIDOutput.
33 *
34 * This feedback controller runs in discrete time, so time deltas are not used
35 * in the integral and derivative calculations. Therefore, the sample rate
36 * affects the controller's behavior for a given set of PID constants.
37 */
38class PIDController : public LiveWindowSendable,
39 public PIDInterface,
40 public ITableListener {
41 public:
42 PIDController(double p, double i, double d, PIDSource* source,
43 PIDOutput* output, double period = 0.05);
44 PIDController(double p, double i, double d, double f, PIDSource* source,
45 PIDOutput* output, double period = 0.05);
46 virtual ~PIDController();
47
48 PIDController(const PIDController&) = delete;
49 PIDController& operator=(const PIDController) = delete;
50
51 virtual double Get() const;
52 virtual void SetContinuous(bool continuous = true);
53 virtual void SetInputRange(double minimumInput, double maximumInput);
54 virtual void SetOutputRange(double minimumOutput, double maximumOutput);
55 void SetPID(double p, double i, double d) override;
56 virtual void SetPID(double p, double i, double d, double f);
57 double GetP() const override;
58 double GetI() const override;
59 double GetD() const override;
60 virtual double GetF() const;
61
62 void SetSetpoint(double setpoint) override;
63 double GetSetpoint() const override;
64 double GetDeltaSetpoint() const;
65
66 virtual double GetError() const;
67 virtual double GetAvgError() const;
68
69 virtual void SetPIDSourceType(PIDSourceType pidSource);
70 virtual PIDSourceType GetPIDSourceType() const;
71
72 virtual void SetTolerance(double percent);
73 virtual void SetAbsoluteTolerance(double absValue);
74 virtual void SetPercentTolerance(double percentValue);
75 virtual void SetToleranceBuffer(int buf = 1);
76 virtual bool OnTarget() const;
77
78 void Enable() override;
79 void Disable() override;
80 bool IsEnabled() const override;
81
82 void Reset() override;
83
84 void InitTable(std::shared_ptr<ITable> subtable) override;
85
86 protected:
87 PIDSource* m_pidInput;
88 PIDOutput* m_pidOutput;
89
90 std::shared_ptr<ITable> m_table;
91 virtual void Calculate();
92 virtual double CalculateFeedForward();
93 double GetContinuousError(double error) const;
94
95 private:
96 // factor for "proportional" control
97 double m_P;
98 // factor for "integral" control
99 double m_I;
100 // factor for "derivative" control
101 double m_D;
102 // factor for "feed forward" control
103 double m_F;
104 // |maximum output|
105 double m_maximumOutput = 1.0;
106 // |minimum output|
107 double m_minimumOutput = -1.0;
108 // maximum input - limit setpoint to this
109 double m_maximumInput = 0;
110 // minimum input - limit setpoint to this
111 double m_minimumInput = 0;
112 // do the endpoints wrap around? eg. Absolute encoder
113 bool m_continuous = false;
114 // is the pid controller enabled
115 bool m_enabled = false;
116 // the prior error (used to compute velocity)
117 double m_prevError = 0;
118 // the sum of the errors for use in the integral calc
119 double m_totalError = 0;
120 enum {
121 kAbsoluteTolerance,
122 kPercentTolerance,
123 kNoTolerance
124 } m_toleranceType = kNoTolerance;
125
126 // the percetage or absolute error that is considered on target.
127 double m_tolerance = 0.05;
128 double m_setpoint = 0;
129 double m_prevSetpoint = 0;
130 double m_error = 0;
131 double m_result = 0;
132 double m_period;
133
134 // Length of buffer for averaging for tolerances.
135 std::atomic<unsigned> m_bufLength{1};
136 std::queue<double> m_buf;
137 double m_bufTotal = 0;
138
139 mutable priority_recursive_mutex m_mutex;
140
141 std::unique_ptr<Notifier> m_controlLoop;
142 Timer m_setpointTimer;
143
144 std::shared_ptr<ITable> GetTable() const override;
145 std::string GetSmartDashboardType() const override;
146 void ValueChanged(ITable* source, llvm::StringRef key,
147 std::shared_ptr<nt::Value> value, bool isNew) override;
148 void UpdateTable() override;
149 void StartLiveWindowMode() override;
150 void StopLiveWindowMode() override;
151};
152
153} // namespace frc