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

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

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