Check that subsystem range is within pot voltage range
This checks that the subsystem range is within the potentiometer
voltage range, ensuring that the potentiometer won't break, even
at range extremes. The range that the potentiometer can handle
is 0 to 5 volts or -5 to 0 volts, so the subsystem positional
limits should be within these bounds.
Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: I3c7f06595541408111b4c393c6610acb635d4b07
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 798a561..eb0571c 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -440,6 +440,35 @@
],
)
+cc_library(
+ name = "wpilib_utils",
+ srcs = [
+ "wpilib_utils.cc",
+ ],
+ hdrs = [
+ "wpilib_utils.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//frc971:constants",
+ ],
+)
+
+cc_test(
+ name = "wpilib_utils_test",
+ srcs = [
+ "wpilib_utils_test.cc",
+ ],
+ data = [
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/testing:flatbuffer_eq",
+ "//aos/testing:test_logging",
+ "//frc971/wpilib:wpilib_utils",
+ ],
+)
+
ts_library(
name = "imu_plot_utils",
srcs = ["imu_plot_utils.ts"],
diff --git a/frc971/wpilib/wpilib_utils.cc b/frc971/wpilib/wpilib_utils.cc
new file mode 100644
index 0000000..937acd9
--- /dev/null
+++ b/frc971/wpilib/wpilib_utils.cc
@@ -0,0 +1,26 @@
+#include "frc971/wpilib/wpilib_utils.h"
+
+namespace frc971 {
+namespace wpilib {
+
+bool SafePotVoltageRange(::frc971::constants::Range subsystem_range,
+ double potentiometer_offset,
+ ::std::function<double(double)> pot_translate_inverse,
+ bool reverse, double limit_buffer) {
+ constexpr double kMinVoltage = 0.0;
+ constexpr double kMaxVoltage = 5.0;
+ double min_range_voltage =
+ pot_translate_inverse(subsystem_range.lower_hard - potentiometer_offset);
+ double max_range_voltage =
+ pot_translate_inverse(subsystem_range.upper_hard - potentiometer_offset);
+ if (reverse) {
+ min_range_voltage *= -1;
+ max_range_voltage *= -1;
+ }
+ return ((kMinVoltage + limit_buffer) < min_range_voltage &&
+ min_range_voltage < (kMaxVoltage - limit_buffer) &&
+ (kMinVoltage + limit_buffer) < max_range_voltage &&
+ max_range_voltage < (kMaxVoltage - limit_buffer));
+}
+} // namespace wpilib
+} // namespace frc971
\ No newline at end of file
diff --git a/frc971/wpilib/wpilib_utils.h b/frc971/wpilib/wpilib_utils.h
new file mode 100644
index 0000000..78dd262
--- /dev/null
+++ b/frc971/wpilib/wpilib_utils.h
@@ -0,0 +1,28 @@
+#ifndef FRC971_WPILIB_WPILIB_UTILS_H_
+#define FRC971_WPILIB_WPILIB_UTILS_H_
+
+#include <functional>
+
+#include "frc971/constants.h"
+
+namespace frc971 {
+namespace wpilib {
+
+// Convert min and max angle positions from range to voltage and compare to
+// min and max potentiometer voltage to check if in range.
+
+// subsystem_range is a ::frc971::constants::Range that defines the pot limits
+// potentiometer_offset is a constant that is the initial offset of the pot
+// pot_translate_inverse is a function that translates an angle to voltage
+// reverse is a boolean that sets the pot voltage range as -5 to 0 when true
+// limit_buffer is a constant that is the buffer for the maximum voltage values
+
+bool SafePotVoltageRange(::frc971::constants::Range subsystem_range,
+ double potentiometer_offset,
+ ::std::function<double(double)> pot_translate_inverse,
+ bool reverse, double limit_buffer = 0.05);
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_WPILIB_UTILS_H_
\ No newline at end of file
diff --git a/frc971/wpilib/wpilib_utils_test.cc b/frc971/wpilib/wpilib_utils_test.cc
new file mode 100644
index 0000000..2b21438
--- /dev/null
+++ b/frc971/wpilib/wpilib_utils_test.cc
@@ -0,0 +1,158 @@
+#include "frc971/wpilib/wpilib_utils.h"
+
+#include "aos/testing/flatbuffer_eq.h"
+#include "aos/testing/test_logging.h"
+#include "frc971/constants.h"
+
+namespace frc971 {
+namespace wpilib {
+namespace testing {
+namespace {
+
+double climber_pot_translate_inverse_test(double position) {
+ return position * 10.0;
+}
+
+double intake_pot_translate_inverse_test(double position) {
+ return position * 2.0;
+}
+
+TEST(WpilibUtilsTest, ZeroOffsetZeroBuffer) {
+ {
+ // Physical pot range: 0.0 to 0.5
+ constexpr double kPotentiometerOffset = 0.00;
+ constexpr double kLimitBuffer = 0.00;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = 0.01, .upper_hard = 0.49, .lower = 0.1, .upper = 0.4};
+ EXPECT_TRUE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ climber_pot_translate_inverse_test, false,
+ kLimitBuffer));
+ }
+ {
+ // Physical pot range: 0.0 to 0.5
+ constexpr double kPotentiometerOffset = 0.00;
+ constexpr double kLimitBuffer = 0.00;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = 0.0, .upper_hard = 0.5, .lower = 0.1, .upper = 0.4};
+ EXPECT_FALSE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ climber_pot_translate_inverse_test, false,
+ kLimitBuffer));
+ }
+}
+
+TEST(WpilibUtilsTest, LimitBuffer) {
+ {
+ // Physical pot range: 0.0 to 0.5
+ constexpr double kPotentiometerOffset = 0.0;
+ constexpr double kLimitBuffer = 0.00;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = 0.005, .upper_hard = 0.495, .lower = 0.1, .upper = 0.4};
+ EXPECT_TRUE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ climber_pot_translate_inverse_test, false,
+ kLimitBuffer));
+ }
+ {
+ // Physical pot range: 0.0 to 0.5, with limit buffer: 0.005 to 0.495
+ constexpr double kPotentiometerOffset = 0.0;
+ constexpr double kLimitBuffer = 0.05;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = 0.005, .upper_hard = 0.495, .lower = 0.1, .upper = 0.4};
+ EXPECT_FALSE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ climber_pot_translate_inverse_test, false,
+ kLimitBuffer));
+ }
+}
+
+TEST(WpilibUtilsTest, PotOffset) {
+ {
+ // Physical pot range: 0.0 to 0.5
+ constexpr double kPotentiometerOffset = 0.0;
+ constexpr double kLimitBuffer = 0.00;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = 0.01, .upper_hard = 0.49, .lower = 0.1, .upper = 0.4};
+ EXPECT_TRUE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ climber_pot_translate_inverse_test, false,
+ kLimitBuffer));
+ }
+ {
+ // Physical pot range: 0.1 to 0.6
+ constexpr double kPotentiometerOffset = 0.1;
+ constexpr double kLimitBuffer = 0.00;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = 0.01, .upper_hard = 0.49, .lower = 0.1, .upper = 0.4};
+ EXPECT_FALSE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ climber_pot_translate_inverse_test, false,
+ kLimitBuffer));
+ }
+}
+
+TEST(WpilibUtilsTest, TranslateFunction) {
+ {
+ // Physical pot range: 0.0 to 2.5
+ constexpr double kPotentiometerOffset = 0.0;
+ constexpr double kLimitBuffer = 0.00;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = 0.01, .upper_hard = 2.49, .lower = 0.1, .upper = 2.4};
+ EXPECT_TRUE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ intake_pot_translate_inverse_test, false,
+ kLimitBuffer));
+ }
+ {
+ // Physical pot range: 0.0 to 0.5
+ constexpr double kPotentiometerOffset = 0.0;
+ constexpr double kLimitBuffer = 0.00;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = 0.01, .upper_hard = 2.49, .lower = 0.1, .upper = 2.4};
+ EXPECT_FALSE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ climber_pot_translate_inverse_test, false,
+ kLimitBuffer));
+ }
+}
+
+TEST(WpilibUtilsTest, ReverseRange) {
+ {
+ // Physical pot range: 0.0 to 0.5, reversed: -0.5 to 0.0
+ constexpr double kPotentiometerOffset = 0.0;
+ constexpr double kLimitBuffer = 0.00;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = -0.49, .upper_hard = -0.01, .lower = -0.4, .upper = -0.1};
+ EXPECT_TRUE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ climber_pot_translate_inverse_test, true,
+ kLimitBuffer));
+ }
+ {
+ // Physical pot range: 0.0 to 0.5
+ constexpr double kPotentiometerOffset = 0.0;
+ constexpr double kLimitBuffer = 0.00;
+
+ ::frc971::constants::Range subsystem_range;
+ subsystem_range = ::frc971::constants::Range{
+ .lower_hard = -0.49, .upper_hard = -0.01, .lower = -0.4, .upper = -0.1};
+ EXPECT_FALSE(SafePotVoltageRange(subsystem_range, kPotentiometerOffset,
+ climber_pot_translate_inverse_test, false,
+ kLimitBuffer));
+ }
+}
+
+} // namespace
+} // namespace testing
+} // namespace wpilib
+} // namespace frc971
\ No newline at end of file