1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <limits>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "wpimath/MathShared.h"
namespace frc {
/**
* Implements a bang-bang controller, which outputs either 0 or 1 depending on
* whether the measurement is less than the setpoint. This maximally-aggressive
* control approach works very well for velocity control of high-inertia
* mechanisms, and poorly on most other things.
*
* <p>Note that this is an *asymmetric* bang-bang controller - it will not exert
* any control effort in the reverse direction (e.g. it won't try to slow down
* an over-speeding shooter wheel). This asymmetry is *extremely important.*
* Bang-bang control is extremely simple, but also potentially hazardous. Always
* ensure that your motor controllers are set to "coast" before attempting to
* control them with a bang-bang controller.
*/
class WPILIB_DLLEXPORT BangBangController
: public wpi::Sendable,
public wpi::SendableHelper<BangBangController> {
public:
/**
* Creates a new bang-bang controller.
*
* <p>Always ensure that your motor controllers are set to "coast" before
* attempting to control them with a bang-bang controller.
*
* @param tolerance Tolerance for atSetpoint.
*/
constexpr explicit BangBangController(
double tolerance = std::numeric_limits<double>::infinity())
: m_tolerance(tolerance) {
if (!std::is_constant_evaluated()) {
++instances;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kController_BangBangController, instances);
}
}
/**
* Sets the setpoint for the bang-bang controller.
*
* @param setpoint The desired setpoint.
*/
constexpr void SetSetpoint(double setpoint) { m_setpoint = setpoint; }
/**
* Returns the current setpoint of the bang-bang controller.
*
* @return The current setpoint.
*/
constexpr double GetSetpoint() const { return m_setpoint; }
/**
* Returns true if the error is within the tolerance of the setpoint.
*
* @return Whether the error is within the acceptable bounds.
*/
constexpr bool AtSetpoint() const {
return gcem::abs(m_setpoint - m_measurement) < m_tolerance;
}
/**
* Sets the error within which AtSetpoint will return true.
*
* @param tolerance Position error which is tolerable.
*/
constexpr void SetTolerance(double tolerance) { m_tolerance = tolerance; }
/**
* Returns the current tolerance of the controller.
*
* @return The current tolerance.
*/
constexpr double GetTolerance() const { return m_tolerance; }
/**
* Returns the current measurement of the process variable.
*
* @return The current measurement of the process variable.
*/
constexpr double GetMeasurement() const { return m_measurement; }
/**
* Returns the current error.
*
* @return The current error.
*/
constexpr double GetError() const { return m_setpoint - m_measurement; }
/**
* Returns the calculated control output.
*
* <p>Always ensure that your motor controllers are set to "coast" before
* attempting to control them with a bang-bang controller.
*
* @param measurement The most recent measurement of the process variable.
* @param setpoint The setpoint for the process variable.
* @return The calculated motor output (0 or 1).
*/
constexpr double Calculate(double measurement, double setpoint) {
m_measurement = measurement;
m_setpoint = setpoint;
return measurement < setpoint ? 1 : 0;
}
/**
* Returns the calculated control output.
*
* @param measurement The most recent measurement of the process variable.
* @return The calculated motor output (0 or 1).
*/
constexpr double Calculate(double measurement) {
return Calculate(measurement, m_setpoint);
}
void InitSendable(wpi::SendableBuilder& builder) override;
private:
double m_tolerance;
double m_setpoint = 0;
double m_measurement = 0;
// Usage reporting instances
inline static int instances = 0;
};
} // namespace frc