Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 26e4e5289bfe3b1691be9c14bedfb70bdcd6dc56 / . / wpilibc / shared / include / PIDController.h
blob: 301356cbb3cfc6a41e772462ef7a76997f15970c [file] [log] [blame]
Brian Silverman26e4e522015-12-17 01:56:40 -0500[diff] [blame^]1/*----------------------------------------------------------------------------*/
2/* Copyright (c) FIRST 2008. All Rights Reserved.
3 */
4/* Open Source Software - may be modified and shared by FRC teams. The code */
5/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
6/*----------------------------------------------------------------------------*/
7#pragma once
8
9#include "Base.h"
10#include "Controller.h"
11#include "LiveWindow/LiveWindow.h"
12#include "PIDInterface.h"
13#include "PIDSource.h"
14#include "Notifier.h"
15#include "HAL/cpp/priority_mutex.h"
16
17#include <memory>
18
19#include <atomic>
20#include <queue>
21
22class PIDOutput;
23
24/**
25 * Class implements a PID Control Loop.
26 *
27 * Creates a separate thread which reads the given PIDSource and takes
28 * care of the integral calculations, as well as writing the given
29 * PIDOutput
30 */
31class PIDController : public LiveWindowSendable,
32 public PIDInterface,
33 public ITableListener {
34 public:
35 PIDController(float p, float i, float d, PIDSource *source, PIDOutput *output,
36 float period = 0.05);
37 PIDController(float p, float i, float d, float f, PIDSource *source,
38 PIDOutput *output, float period = 0.05);
39 virtual ~PIDController();
40
41 PIDController(const PIDController&) = delete;
42 PIDController& operator=(const PIDController) = delete;
43
44 virtual float Get() const;
45 virtual void SetContinuous(bool continuous = true);
46 virtual void SetInputRange(float minimumInput, float maximumInput);
47 virtual void SetOutputRange(float minimumOutput, float maximumOutput);
48 virtual void SetPID(double p, double i, double d) override;
49 virtual void SetPID(double p, double i, double d, double f);
50 virtual double GetP() const override;
51 virtual double GetI() const override;
52 virtual double GetD() const override;
53 virtual double GetF() const;
54
55 virtual void SetSetpoint(float setpoint) override;
56 virtual double GetSetpoint() const override;
57
58 virtual float GetError() const;
59 virtual float GetAvgError() const;
60
61 virtual void SetPIDSourceType(PIDSourceType pidSource);
62 virtual PIDSourceType GetPIDSourceType() const;
63
64 virtual void SetTolerance(float percent);
65 virtual void SetAbsoluteTolerance(float absValue);
66 virtual void SetPercentTolerance(float percentValue);
67 virtual void SetToleranceBuffer(unsigned buf = 1);
68 virtual bool OnTarget() const;
69
70 virtual void Enable() override;
71 virtual void Disable() override;
72 virtual bool IsEnabled() const override;
73
74 virtual void Reset() override;
75
76 virtual void InitTable(std::shared_ptr<ITable> table) override;
77
78 protected:
79 PIDSource *m_pidInput;
80 PIDOutput *m_pidOutput;
81
82 std::shared_ptr<ITable> m_table;
83 virtual void Calculate();
84
85 private:
86 float m_P; // factor for "proportional" control
87 float m_I; // factor for "integral" control
88 float m_D; // factor for "derivative" control
89 float m_F; // factor for "feed forward" control
90 float m_maximumOutput = 1.0; // |maximum output|
91 float m_minimumOutput = -1.0; // |minimum output|
92 float m_maximumInput = 0; // maximum input - limit setpoint to this
93 float m_minimumInput = 0; // minimum input - limit setpoint to this
94 bool m_continuous = false; // do the endpoints wrap around? eg. Absolute encoder
95 bool m_enabled = false; // is the pid controller enabled
96 float m_prevInput = 0; // the prior sensor input (used to compute velocity)
97 double m_totalError = 0; // the sum of the errors for use in the integral calc
98 enum {
99 kAbsoluteTolerance,
100 kPercentTolerance,
101 kNoTolerance
102 } m_toleranceType = kNoTolerance;
103
104 // the percetage or absolute error that is considered on target.
105 float m_tolerance = 0.05;
106 float m_setpoint = 0;
107 float m_error = 0;
108 float m_result = 0;
109 float m_period;
110
111 // Length of buffer for averaging for tolerances.
112 std::atomic<unsigned> m_bufLength{1};
113 std::queue<double> m_buf;
114 double m_bufTotal = 0;
115
116 mutable priority_mutex m_mutex;
117
118 std::unique_ptr<Notifier> m_controlLoop;
119
120 void Initialize(float p, float i, float d, float f, PIDSource *source,
121 PIDOutput *output, float period = 0.05);
122 static void CallCalculate(void *controller);
123
124 virtual std::shared_ptr<ITable> GetTable() const override;
125 virtual std::string GetSmartDashboardType() const override;
126 virtual void ValueChanged(ITable *source, llvm::StringRef key,
127 std::shared_ptr<nt::Value> value,
128 bool isNew) override;
129 virtual void UpdateTable() override;
130 virtual void StartLiveWindowMode() override;
131 virtual void StopLiveWindowMode() override;
132};