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/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