From f90fec5191ff2a84213e666c1dad7b7d15fbd51b Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sun, 7 Jan 2024 19:40:32 +0800
Subject: [PATCH] [wombat/utils] add LimitVoltage function (#62)

---
 wombat/src/main/cpp/utils/Util.cpp                | 10 ++++++++++
 wombat/src/main/include/utils/Encoder.h           |  2 --
 wombat/src/main/include/utils/Pathplanner.h       |  2 --
 wombat/src/main/include/utils/Util.h              |  3 +++
 wombat/src/main/include/utils/VoltageController.h |  2 +-
 5 files changed, 14 insertions(+), 5 deletions(-)

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