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. + */ +public class BangBangController implements Sendable { + private static int instances; + + private double m_tolerance; + + private double m_setpoint; + private double m_measurement; + + /** + * Creates a new bang-bang controller. + * + *
Always ensure that your motor controllers are set to "coast" before attempting to control + * them with a bang-bang controller. + * + * @param tolerance Tolerance for {@link #atSetpoint() atSetpoint}. + */ + public BangBangController(double tolerance) { + instances++; + + setTolerance(tolerance); + + SendableRegistry.addLW(this, "BangBangController", instances); + + MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances); + } + + /** + * Creates a new bang-bang controller. + * + *
Always ensure that your motor controllers are set to "coast" before attempting to control + * them with a bang-bang controller. + */ + public BangBangController() { + this(Double.POSITIVE_INFINITY); + } + + /** + * Sets the setpoint for the bang-bang controller. + * + * @param setpoint The desired setpoint. + */ + public void setSetpoint(double setpoint) { + m_setpoint = setpoint; + } + + /** + * Returns the current setpoint of the bang-bang controller. + * + * @return The current setpoint. + */ + public double getSetpoint() { + return m_setpoint; + } + + /** + * Returns true if the error is within the tolerance of the setpoint. + * + * @return Whether the error is within the acceptable bounds. + */ + public boolean atSetpoint() { + return Math.abs(m_setpoint - m_measurement) < m_tolerance; + } + + /** + * Sets the error within which atSetpoint will return true. + * + * @param tolerance Position error which is tolerable. + */ + public void setTolerance(double tolerance) { + m_tolerance = tolerance; + } + + /** + * Returns the current tolerance of the controller. + * + * @return The current tolerance. + */ + public double getTolerance() { + return m_tolerance; + } + + /** + * Returns the current measurement of the process variable. + * + * @return The current measurement of the process variable. + */ + public double getMeasurement() { + return m_measurement; + } + + /** + * Returns the current error. + * + * @return The current error. + */ + public double getError() { + return m_setpoint - m_measurement; + } + + /** + * Returns the calculated control output. + * + *
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).
+ */
+ public 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).
+ */
+ public double calculate(double measurement) {
+ return calculate(measurement, m_setpoint);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ builder.setSmartDashboardType("BangBangController");
+ builder.addDoubleProperty("tolerance", this::getTolerance, this::setTolerance);
+ builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+ builder.addDoubleProperty("measurement", this::getMeasurement, null);
+ builder.addDoubleProperty("error", this::getError, null);
+ builder.addBooleanProperty("atSetpoint", this::atSetpoint, null);
+ }
+}
diff --git a/wpimath/src/main/native/cpp/controller/BangBangController.cpp b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
new file mode 100644
index 0000000000..a4fda0a673
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
@@ -0,0 +1,72 @@
+// 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.
+
+#include "../../include/frc/controller/BangBangController.h"
+
+#include 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 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.
+ */
+ explicit BangBangController(
+ double tolerance = std::numeric_limits 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).
+ */
+ double Calculate(double measurement, double setpoint);
+
+ /**
+ * Returns the calculated control output.
+ *
+ * @param measurement The most recent measurement of the process variable.
+ * @return The calculated motor output (0 or 1).
+ */
+ double Calculate(double measurement);
+
+ void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+ double m_tolerance;
+
+ double m_setpoint = 0;
+ double m_measurement = 0;
+};
+
+} // namespace frc
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangInputOutputTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangInputOutputTest.java
new file mode 100644
index 0000000000..3621ea3e0d
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangInputOutputTest.java
@@ -0,0 +1,29 @@
+// 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.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+public class BangBangInputOutputTest {
+ private BangBangController m_controller;
+
+ @BeforeEach
+ void setUp() {
+ m_controller = new BangBangController();
+ }
+
+ @Test
+ void shouldOutput() {
+ assertEquals(m_controller.calculate(0, 1), 1);
+ }
+
+ @Test
+ void shouldNotOutput() {
+ assertEquals(m_controller.calculate(1, 0), 0);
+ }
+}
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangToleranceTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangToleranceTest.java
new file mode 100644
index 0000000000..8af90542da
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/controller/BangBangToleranceTest.java
@@ -0,0 +1,34 @@
+// 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.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+public class BangBangToleranceTest {
+ private BangBangController m_controller;
+
+ @BeforeEach
+ void setUp() {
+ m_controller = new BangBangController(0.1);
+ }
+
+ @Test
+ void inTolerance() {
+ m_controller.setSetpoint(1);
+ m_controller.calculate(1);
+ assertTrue(m_controller.atSetpoint());
+ }
+
+ @Test
+ void outOfTolerance() {
+ m_controller.setSetpoint(1);
+ m_controller.calculate(0);
+ assertFalse(m_controller.atSetpoint());
+ }
+}
diff --git a/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
new file mode 100644
index 0000000000..4dfaa0e313
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp
@@ -0,0 +1,19 @@
+// 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.
+
+#include "frc/controller/BangBangController.h"
+#include "gtest/gtest.h"
+
+class BangBangInputOutputTest : public testing::Test {
+ protected:
+ frc::BangBangController controller;
+};
+
+TEST_F(BangBangInputOutputTest, ShouldOutput) {
+ EXPECT_DOUBLE_EQ(controller.Calculate(0, 1), 1);
+}
+
+TEST_F(BangBangInputOutputTest, ShouldNotOutput) {
+ EXPECT_DOUBLE_EQ(controller.Calculate(1, 0), 0);
+}
diff --git a/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
new file mode 100644
index 0000000000..e58272043f
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp
@@ -0,0 +1,23 @@
+// 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.
+
+#include "frc/controller/BangBangController.h"
+#include "gtest/gtest.h"
+
+class BangBangToleranceTest : public testing::Test {
+ protected:
+ frc::BangBangController controller{0.1};
+};
+
+TEST_F(BangBangToleranceTest, InTolerance) {
+ controller.SetSetpoint(1);
+ controller.Calculate(1);
+ EXPECT_TRUE(controller.AtSetpoint());
+}
+
+TEST_F(BangBangToleranceTest, OutOfTolerance) {
+ controller.SetSetpoint(1);
+ controller.Calculate(0);
+ EXPECT_FALSE(controller.AtSetpoint());
+}