[wpilib] Add bang-bang controller (#3676)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java

@@ -0,0 +1,159 @@
+// 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 edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * 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.
+ */
+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.
+   *
+   * <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 {@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.
+   *
+   * <p>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.
+   *
+   * <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).
+   */
+  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);
+  }
+}

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 <wpi/sendable/SendableBuilder.h>
+
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+BangBangController::BangBangController(double tolerance)
+    : m_tolerance(tolerance) {
+  static int instances = 0;
+  instances++;
+}
+
+void BangBangController::SetSetpoint(double setpoint) {
+  m_setpoint = setpoint;
+}
+
+double BangBangController::GetSetpoint() const {
+  return m_setpoint;
+}
+
+bool BangBangController::AtSetpoint() const {
+  return std::abs(m_setpoint - m_measurement) < m_tolerance;
+}
+
+void BangBangController::SetTolerance(double tolerance) {
+  m_tolerance = tolerance;
+}
+
+double BangBangController::GetTolerance() const {
+  return m_tolerance;
+}
+
+double BangBangController::GetMeasurement() const {
+  return m_measurement;
+}
+
+double BangBangController::GetError() const {
+  return m_setpoint - m_measurement;
+}
+
+double BangBangController::Calculate(double measurement, double setpoint) {
+  m_measurement = measurement;
+  m_setpoint = setpoint;
+
+  return measurement < setpoint ? 1 : 0;
+}
+
+double BangBangController::Calculate(double measurement) {
+  return Calculate(measurement, m_setpoint);
+}
+
+void BangBangController::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("BangBangController");
+  builder.AddDoubleProperty(
+      "tolerance", [this] { return GetTolerance(); },
+      [this](double tolerance) { SetTolerance(tolerance); });
+  builder.AddDoubleProperty(
+      "setpoint", [this] { return GetSetpoint(); },
+      [this](double setpoint) { SetSetpoint(setpoint); });
+  builder.AddDoubleProperty(
+      "measurement", [this] { return GetMeasurement(); }, nullptr);
+  builder.AddDoubleProperty(
+      "error", [this] { return GetError(); }, nullptr);
+  builder.AddBooleanProperty(
+      "atSetpoint", [this] { return AtSetpoint(); }, nullptr);
+}

wpimath/src/main/native/include/frc/controller/BangBangController.h

@@ -0,0 +1,121 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.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.
+   */
+  explicit BangBangController(
+      double tolerance = std::numeric_limits<double>::infinity());
+
+  /**
+   * Sets the setpoint for the bang-bang controller.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  void SetSetpoint(double setpoint);
+
+  /**
+   * Returns the current setpoint of the bang-bang controller.
+   *
+   * @return The current setpoint.
+   */
+  double GetSetpoint() const;
+
+  /**
+   * Returns true if the error is within the tolerance of the setpoint.
+   *
+   * @return Whether the error is within the acceptable bounds.
+   */
+  bool AtSetpoint() const;
+
+  /**
+   * Sets the error within which AtSetpoint will return true.
+   *
+   * @param tolerance Position error which is tolerable.
+   */
+  void SetTolerance(double tolerance);
+
+  /**
+   * Returns the current tolerance of the controller.
+   *
+   * @return The current tolerance.
+   */
+  double GetTolerance() const;
+
+  /**
+   * Returns the current measurement of the process variable.
+   *
+   * @return The current measurement of the process variable.
+   */
+  double GetMeasurement() const;
+
+  /**
+   * Returns the current error.
+   *
+   * @return The current error.
+   */
+  double GetError() const;
+
+  /**
+   * 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).
+   */
+  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

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);
+  }
+}

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());
+  }
+}

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);
+}

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());
+}