[wombat/utils] Allow template magic for util math

CurtinFRC · Mar 8, 2024 · 2e4925f · 2e4925f
1 parent 6dc7b68
commit 2e4925f
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 6 deletions.
diff --git a/wombat/.glass/glass.json b/wombat/.glass/glass.json
@@ -0,0 +1,5 @@
+{
+  "NetworkTables Info": {
+    "visible": true
+  }
+}
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
@@ -27,12 +27,14 @@ void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose
   table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert<units::degree>().value());
 }
 
-double wom::utils::deadzone(double val, double deadzone) {
-  return std::fabs(val) > deadzone ? val : 0;
+template <typename T>
+T wom::utils::deadzone(T val, T deadzone) {
+  return std::fabs(static_cast<double>(val)) > static_cast<double>(deadzone) ? val : static_cast<T>(0);
 }
 
-double wom::utils::spow2(double val) {
-  return val * val * (val > 0 ? 1 : -1);
+template <typename T>
+T wom::utils::spow2(T val) {
+  return val * val * (val > static_cast<T>(0) ? 1 : -1);
 }
 
 units::volt_t wom::utils::LimitVoltage(units::volt_t voltage) {

diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
@@ -86,8 +86,10 @@ void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajecto
 
 frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 
-double deadzone(double val, double deadzone = 0.05);
-double spow2(double val);
+template<typename T>
+T deadzone(T val, T deadzone = 0.05);
+template<typename T>
+T spow2(T val);
 
 units::volt_t LimitVoltage(units::volt_t voltage);
 units::volt_t GetVoltage(frc::MotorController* controller);