diff --git a/lib/cpp/subzero/drivetrain/SwerveUtils.cpp b/lib/cpp/subzero/drivetrain/SwerveUtils.cpp
new file mode 100644
index 0000000..6360ffd
--- /dev/null
+++ b/lib/cpp/subzero/drivetrain/SwerveUtils.cpp
@@ -0,0 +1,70 @@
+#include "subzero/drivetrain/SwerveUtils.h"
+
+#include <cmath>
+#include <numbers>
+
+using namespace subzero;
+
+double SwerveUtils::StepTowards(double current, double target,
+                                double stepsize) {
+  if (abs(current - target) <= stepsize) {
+    return target;
+  } else if (target < current) {
+    return current - stepsize;
+  }
+  return current + stepsize;
+}
+
+double SwerveUtils::StepTowardsCircular(double current, double target,
+                                        double stepsize) {
+  current = WrapAngle(current);
+  target = WrapAngle(target);
+
+  double temp = target - current;
+  double stepDirection = temp > 0 ? 1 : temp < 0 ? -1 : 0;
+  double difference = abs(current - target);
+
+  if (difference <= stepsize) {
+    return target;
+  } else if (difference > std::numbers::pi) {
+    // Handles the case where the difference between the two angles is shorter
+    // is less than stepsize when being measured in the other direction
+    if (current + 2 * std::numbers::pi - target < stepsize ||
+        target + 2 * std::numbers::pi - current < stepsize) {
+      return target;
+    }
+    return WrapAngle(current - stepDirection * stepsize);
+  }
+  // If the difference is less than pi yet still greater than the step size,
+  // we just add stepsize to current
+  return current + stepDirection * stepsize;
+}
+
+double SwerveUtils::AngleDifference(double angleA, double angleB) {
+  double difference = abs(angleA - angleB);
+  return difference > std::numbers::pi ? (2 * std::numbers::pi) - difference
+                                       : difference;
+}
+
+double SwerveUtils::WrapAngle(double angle) {
+  double twoPi = 2 * std::numbers::pi;
+
+  if (angle == twoPi) {
+    // This case must be handled seperately to avoid floating point errors with
+    // the floor after division
+    return 0.0;
+  } else if (angle > twoPi) {
+    double rotations = floor(angle / twoPi);
+    // Gets the rotations back into radians and then subtracts angle by the
+    // amount of full rotations
+    return angle - twoPi * rotations;
+  } else if (angle < 0.0) {
+    // Must negate the angle which turns it into a positive value. One more
+    // rotation is added just in case the result of the floor is zero. Since the
+    // _angle is negative, adding it has the effect of subtracting it from those
+    // rotations, giving us a valid positive rotation
+    double rotations = floor((-angle) / twoPi) + 1;
+    return angle + twoPi * rotations;
+  }
+  return angle;
+}
\ No newline at end of file
diff --git a/lib/include/subzero/drivetrain/SwerveUtils.h b/lib/include/subzero/drivetrain/SwerveUtils.h
index 569fcd1..b76bb68 100644
--- a/lib/include/subzero/drivetrain/SwerveUtils.h
+++ b/lib/include/subzero/drivetrain/SwerveUtils.h
@@ -6,50 +6,50 @@ class SwerveUtils {
   /**
    * Steps a value towards a target with a specified step size.
    *
-   * @param _current The current or starting value.  Can be positive or
+   * @param current The current or starting value.  Can be positive or
    * negative.
-   * @param _target The target value the algorithm will step towards.  Can be
+   * @param target The target value the algorithm will step towards.  Can be
    * positive or negative.
-   * @param _stepsize The maximum step size that can be taken.
+   * @param stepsize The maximum step size that can be taken.
    * @return The new value for {@code _current} after performing the specified
    * step towards the specified target.
    */
-  static double StepTowards(double _current, double _target, double _stepsize);
+  static double StepTowards(double current, double target, double stepsize);
 
   /**
    * Steps a value (angle) towards a target (angle) taking the shortest path
    * with a specified step size.
    *
-   * @param _current The current or starting angle (in radians).  Can lie
+   * @param current The current or starting angle (in radians).  Can lie
    * outside the 0 to 2*PI range.
-   * @param _target The target angle (in radians) the algorithm will step
+   * @param target The target angle (in radians) the algorithm will step
    * towards.  Can lie outside the 0 to 2*PI range.
-   * @param _stepsize The maximum step size that can be taken (in radians).
+   * @param stepsize The maximum step size that can be taken (in radians).
    * @return The new angle (in radians) for {@code _current} after performing
    * the specified step towards the specified target. This value will always lie
    * in the range 0 to 2*PI (exclusive).
    */
-  static double StepTowardsCircular(double _current, double _target,
-                                    double _stepsize);
+  static double StepTowardsCircular(double current, double target,
+                                    double stepsize);
 
   /**
    * Finds the (unsigned) minimum difference between two angles including
    * calculating across 0.
    *
-   * @param _angleA An angle (in radians).
-   * @param _angleB An angle (in radians).
+   * @param angleA An angle (in radians).
+   * @param angleB An angle (in radians).
    * @return The (unsigned) minimum difference between the two angles (in
    * radians).
    */
-  static double AngleDifference(double _angleA, double _angleB);
+  static double AngleDifference(double angleA, double angleB);
 
   /**
    * Wraps an angle until it lies within the range from 0 to 2*PI (exclusive).
    *
-   * @param _angle The angle (in radians) to wrap.  Can be positive or negative
+   * @param angle The angle (in radians) to wrap.  Can be positive or negative
    * and can lie multiple wraps outside the output range.
    * @return An angle (in radians) from 0 and 2*PI (exclusive).
    */
-  static double WrapAngle(double _angle);
+  static double WrapAngle(double angle);
 };
 } // namespace subzero
\ No newline at end of file