From da90c1cd2c3830dfa6b4ae802e5dfe2ca261a0f3 Mon Sep 17 00:00:00 2001 From: Oblarg Date: Tue, 23 Nov 2021 23:34:46 -0500 Subject: [PATCH] [wpilib] Add bang-bang controller (#3676) Co-authored-by: Tyler Veness --- .../math/controller/BangBangController.java | 159 ++++++++++++++++++ .../cpp/controller/BangBangController.cpp | 72 ++++++++ .../frc/controller/BangBangController.h | 121 +++++++++++++ .../controller/BangBangInputOutputTest.java | 29 ++++ .../controller/BangBangToleranceTest.java | 34 ++++ .../controller/BangBangInputOutputTest.cpp | 19 +++ .../cpp/controller/BangBangToleranceTest.cpp | 23 +++ 7 files changed, 457 insertions(+) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java create mode 100644 wpimath/src/main/native/cpp/controller/BangBangController.cpp create mode 100644 wpimath/src/main/native/include/frc/controller/BangBangController.h create mode 100644 wpimath/src/test/java/edu/wpi/first/math/controller/BangBangInputOutputTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/controller/BangBangToleranceTest.java create mode 100644 wpimath/src/test/native/cpp/controller/BangBangInputOutputTest.cpp create mode 100644 wpimath/src/test/native/cpp/controller/BangBangToleranceTest.cpp diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/BangBangController.java new file mode 100644 index 0000000000..7e8c7786bb --- /dev/null +++ b/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. + * + *

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 + +#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); +} diff --git a/wpimath/src/main/native/include/frc/controller/BangBangController.h b/wpimath/src/main/native/include/frc/controller/BangBangController.h new file mode 100644 index 0000000000..6058c81ebc --- /dev/null +++ b/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 + +#include +#include +#include + +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. + * + *

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 { + public: + /** + * 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 atSetpoint. + */ + explicit BangBangController( + double tolerance = std::numeric_limits::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. + * + *

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