[wombat/utils] add LimitVoltage function (#62)

CurtinFRC · Jan 7, 2024 · f90fec5 · f90fec5
1 parent 4784039
commit f90fec5
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 5 deletions.
diff --git 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 <frc/RobotController.h>
+
 units::second_t wom::utils::now() {
   uint64_t now = frc::RobotController::GetFPGATime();
   return static_cast<double>(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
@@ -13,8 +13,6 @@
 
 #include <string>
 
-#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
@@ -8,8 +8,6 @@
 #include <frc/trajectory/TrajectoryUtil.h>
 #include <wpi/fs.h>
 
-#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
@@ -8,6 +8,7 @@
 #include <frc/geometry/Pose3d.h>
 #include <networktables/NetworkTable.h>
 #include <units/time.h>
+#include <units/voltage.h>
 
 #include <functional>
 #include <iostream>
@@ -95,5 +96,7 @@ void WritePose3NT(std::shared_ptr<nt::NetworkTable> 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
@@ -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)
    */