mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-30 02:31:44 +00:00
144 lines
4.3 KiB
C++
144 lines
4.3 KiB
C++
// 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("BangBangController",
|
|
std::to_string(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
|