diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp index 3d3e6e66..271c3b1f 100644 --- a/wombat/src/main/cpp/utils/Util.cpp +++ b/wombat/src/main/cpp/utils/Util.cpp @@ -4,6 +4,8 @@ #include "utils/Util.h" +#include + units::second_t wom::utils::now() { uint64_t now = frc::RobotController::GetFPGATime(); return static_cast(now) / 1000000 * 1_s; @@ -33,3 +35,11 @@ double wom::utils::deadzone(double val, double deadzone) { double wom::utils::spow2(double val) { return val * val * (val > 0 ? 1 : -1); } + +units::volt_t wom::utils::LimitVoltage(units::volt_t voltage) { + if (voltage >= frc::RobotController::GetBatteryVoltage()) { + return frc::RobotController::GetBatteryVoltage() - 0.5_V; + } + + return voltage; +} diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h index 618f7b6f..1f3c166c 100644 --- a/wombat/src/main/include/utils/Encoder.h +++ b/wombat/src/main/include/utils/Encoder.h @@ -13,8 +13,6 @@ #include -#include "utils/Util.h" - namespace wom { namespace utils { class Encoder { diff --git a/wombat/src/main/include/utils/Pathplanner.h b/wombat/src/main/include/utils/Pathplanner.h index 806ea2a3..48e760ae 100644 --- a/wombat/src/main/include/utils/Pathplanner.h +++ b/wombat/src/main/include/utils/Pathplanner.h @@ -8,8 +8,6 @@ #include #include -#include "utils/Util.h" - namespace wom { namespace utils { class Pathplanner { diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h index 42f8b916..fc1c08ca 100644 --- a/wombat/src/main/include/utils/Util.h +++ b/wombat/src/main/include/utils/Util.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -95,5 +96,7 @@ void WritePose3NT(std::shared_ptr table, frc::Pose3d pose); double deadzone(double val, double deadzone = 0.05); double spow2(double val); + +units::volt_t LimitVoltage(units::volt_t voltage); } // namespace utils } // namespace wom diff --git a/wombat/src/main/include/utils/VoltageController.h b/wombat/src/main/include/utils/VoltageController.h index e9079e9a..6f7e19fd 100644 --- a/wombat/src/main/include/utils/VoltageController.h +++ b/wombat/src/main/include/utils/VoltageController.h @@ -44,7 +44,7 @@ class VoltageController { units::volt_t GetBusVoltage() const; /** - * Create a MotorVoltageController with a given frc::MotorController + * Create a VoltageController with a given frc::MotorController * subclass. Please note that this creates an unsafe pointer (will never * dealloc) */