From 68cb56707f97c99c3eec0bc499e5782a20815ffe Mon Sep 17 00:00:00 2001
From: Rong <ronrzhu@gmail.com>
Date: Sat, 18 Jan 2025 15:48:05 -0800
Subject: [PATCH] Revert "Swerve simple trajectory update (#485)"

This reverts commit 8ae3bcbc2a4512529ca40e598c379f1682a50b61.
---
 .../drive/SwerveCalculatorNode.java           |  12 -
 .../drive/SwerveKinematicsCalculator.java     | 288 ------------------
 .../drive/SwervePointKinematics.java          |  28 --
 .../drive/SwerveSimpleTrajectoryCommand.java  |   5 +-
 .../drive/SwerveSimpleTrajectoryMode.java     |   9 -
 .../subsystems/pose/BasePoseSubsystem.java    |   1 -
 .../trajectory/ProvidesInterpolationData.java |   3 -
 .../trajectory/SimpleTimeInterpolator.java    |  62 +---
 .../SwerveSimpleTrajectoryLogic.java          | 191 ++----------
 .../common/trajectory/XbotSwervePoint.java    |   9 -
 .../drive/SwerveCalculatorTest.java           | 125 --------
 11 files changed, 39 insertions(+), 694 deletions(-)
 delete mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java
 delete mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java
 delete mode 100644 src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java
 delete mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java
 delete mode 100644 src/test/java/xbot/common/subsystems/drive/SwerveCalculatorTest.java

diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java b/src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java
deleted file mode 100644
index ebae44dd8..000000000
--- a/src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java
+++ /dev/null
@@ -1,12 +0,0 @@
-package xbot.common.subsystems.drive;
-
-
-/**
- * @param operationEndingVelocity m/s, velocity we end at
- * @param operationAcceleration   mls^2, acceleration to go at
- * @param operationTime           Time in seconds
- */ // SwerveCalculatorNode are small little steps to be interpreted and formed together
-//to create a path to be used for the SwerveKinematicsCalculator
-public record SwerveCalculatorNode(double operationTime, double operationAcceleration, double operationEndingVelocity) {
-
-}
diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java b/src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java
deleted file mode 100644
index bb75971b2..000000000
--- a/src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java
+++ /dev/null
@@ -1,288 +0,0 @@
-package xbot.common.subsystems.drive;
-
-import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;
-import xbot.common.logging.RobotAssertionManager;
-
-import java.util.ArrayList;
-import java.util.List;
-
-public class SwerveKinematicsCalculator {
-
-    /*
-    The SwerveKinematicsCalculator is designed to compute the motion of a swerve drive system from a starting position
-     and velocity to a goal position and velocity. It takes into account parameters like maximum acceleration,
-     goal velocity, and maximum velocity to generate a sequence of motion stages, ensuring precise path planning
-     and execution.
-
-    (Breaks an XbotSwervePoint down into smaller nodes containing time, velocity, and acceleration)
-    Note: goal velocity may not always be reached, but in such case will be as close as possible.
-
-    Code involving the quadratic formula IS NOT ROBUST (since only this script uses them as of currently)
-    */
-
-    final double startPosition;
-    final double endPosition;
-    final double acceleration;
-    final double initialVelocity;
-    final double goalVelocity;
-    final double maxVelocity;
-    final List<SwerveCalculatorNode> nodeMap;
-    RobotAssertionManager assertionManager;
-
-    // Returns the x-intercepts of a quadratic equation
-    private static List<Double> quadraticFormula(double a, double b, double c) {
-        double squareRootResult = Math.sqrt(Math.pow(b, 2) - (4 * a * c));
-        double result1 = (-b + squareRootResult) / (2 * a);
-        double result2 = (-b - squareRootResult) / (2 * a);
-
-        List<Double> results = new ArrayList<>();
-        results.add(result1);
-        results.add(result2);
-        return results;
-    }
-
-    public static double calculateTimeToGoalPosition(double acceleration, double initialVelocity,
-                                                     double initialPosition, double goalPosition) {
-        double a = 0.5 * acceleration;
-        double b = initialVelocity;
-        double c = initialPosition - goalPosition;
-        if (acceleration == 0) {
-            return (-c/b);
-
-        } else if (acceleration > 0) {
-            List<Double> result = quadraticFormula(a, b, c);
-            return Math.max(result.get(0), result.get(1));
-
-        } else {
-            List<Double> result = quadraticFormula(-a, -b, -c);
-            return Math.min(result.get(0), result.get(1));
-
-        }
-    }
-
-    public SwerveKinematicsCalculator(RobotAssertionManager assertionManager, double startPosition, double endPosition,
-                                      SwervePointKinematics kinematics) {
-        this.assertionManager = assertionManager;
-        // Gimmicky way to avoid division by zero and to deal with human code-error
-        if (endPosition - startPosition == 0) {
-            assertionManager.throwException("Calculator instantiated for same start and end position", new Exception());
-            endPosition += 0.01;
-        }
-
-        this.startPosition = startPosition;
-        this.endPosition = endPosition;
-        this.acceleration = kinematics.acceleration;
-        this.initialVelocity = kinematics.initialVelocity;
-        this.goalVelocity = kinematics.goalVelocity;
-        this.maxVelocity = kinematics.maxVelocity;
-
-        // Likely need to validate the above values to prevent bad stuff
-        nodeMap = generateNodeMap();
-    }
-
-    // Based off of given value, initialize the proper path, store notes in a list, and be prepared to output values
-    // (If velocity gets to negative we are *KINDA* cooked)
-    public List<SwerveCalculatorNode> generateNodeMap() {
-        List<SwerveCalculatorNode> nodeMap = new ArrayList<>();
-        double leftoverDistance = endPosition - startPosition;
-
-        double currentVelocity = initialVelocity;
-
-        // Deceleration
-        if (currentVelocity > goalVelocity) {
-            // Cruise as much as possible, then decelerate
-            double decelerateToGoalVelocityTime = (goalVelocity - currentVelocity) / -acceleration;
-            double distanceNeededToDecelerate = 0.5 * (currentVelocity + goalVelocity) * decelerateToGoalVelocityTime;
-
-            double distanceExcludingDeceleration = leftoverDistance - distanceNeededToDecelerate;
-
-            if (distanceExcludingDeceleration > 0) {
-                double halfDistance = distanceExcludingDeceleration / 2;
-                double timeForHalf = calculateTimeToGoalPosition(acceleration, currentVelocity, 0, halfDistance);
-                double peakVelocity = currentVelocity + acceleration * timeForHalf;
-
-                if (peakVelocity <= maxVelocity) {
-                    // Add two nodes, accelerate to peak, decelerate to goal
-                    nodeMap.add(new SwerveCalculatorNode(timeForHalf, acceleration, peakVelocity));
-                    nodeMap.add(new SwerveCalculatorNode(timeForHalf, -acceleration, currentVelocity));
-                } else {
-                    // Going to peak will not work as it will exceed our maximumVelocity limit
-                    // Go to max, then cruise, then decelerate
-                    double timeFromCurrentToMaxV = (maxVelocity - currentVelocity) / acceleration; // Rename
-                    double initialPosition = (endPosition - startPosition) - leftoverDistance;
-                    double finalPositionAfterAcceleration = 0.5 * (currentVelocity + maxVelocity) * timeFromCurrentToMaxV + initialPosition;
-                    double positionDelta = finalPositionAfterAcceleration - initialPosition;
-                    double cruiseDistance = distanceExcludingDeceleration - (positionDelta * 2);
-                    double cruiseTime = cruiseDistance / maxVelocity;
-
-                    nodeMap.add(new SwerveCalculatorNode(timeFromCurrentToMaxV, acceleration, maxVelocity));
-                    nodeMap.add(new SwerveCalculatorNode(cruiseTime, 0, maxVelocity));
-                    nodeMap.add(new SwerveCalculatorNode(timeFromCurrentToMaxV, -acceleration, currentVelocity));
-                }
-
-                nodeMap.add(new SwerveCalculatorNode(decelerateToGoalVelocityTime, -acceleration, goalVelocity));
-
-            } else {
-                // Decelerate as much as possible
-                double decelerationTime = calculateTimeToGoalPosition(-acceleration, currentVelocity, 0, leftoverDistance);
-                nodeMap.add(new SwerveCalculatorNode(
-                        decelerationTime,
-                        -acceleration,
-                        currentVelocity - acceleration * decelerationTime
-                ));
-            }
-            return nodeMap;
-        }
-
-        // Acceleration
-        if (currentVelocity < goalVelocity) {
-            double initialToGoalVelocityTime = (goalVelocity - currentVelocity) / acceleration;
-            double operationDistance = currentVelocity * initialToGoalVelocityTime
-                    + 0.5 * acceleration * Math.pow(initialToGoalVelocityTime, 2);
-
-            if (operationDistance >= leftoverDistance) {
-                // No matter how much you accelerate you won't reach goal velocity before distance...
-                // ...so we accelerate til the end!
-                double time = calculateTimeToGoalPosition(acceleration, currentVelocity, 0, leftoverDistance);
-                nodeMap.add(new SwerveCalculatorNode(
-                        time,
-                        acceleration,
-                        currentVelocity + acceleration * time
-                ));
-                return nodeMap;
-            } else {
-                // Accelerate to goal velocity
-                nodeMap.add(new SwerveCalculatorNode(initialToGoalVelocityTime, acceleration, goalVelocity));
-                leftoverDistance -= operationDistance;
-                currentVelocity = goalVelocity;
-            }
-        }
-
-        // STEP 2: If not at max velocity, build an accelerate->cruise>decelerate nodeMap
-        if (currentVelocity < maxVelocity) {
-            // Check /\ (vortex) does the job (or maybe its peak exceeds maxVelocity)
-            // if not then we need to add a cruising part: /---\ (looks like this)
-            double halfDistance = leftoverDistance / 2;
-            double timeForHalf = calculateTimeToGoalPosition(acceleration, currentVelocity, 0, halfDistance);
-
-            // Figure out end velocity
-            double peakVelocity = currentVelocity + acceleration * timeForHalf;
-            if (peakVelocity <= maxVelocity) {
-                // Add two nodes, accelerate to peak, decelerate to goal
-                nodeMap.add(new SwerveCalculatorNode(timeForHalf, acceleration, peakVelocity));
-                nodeMap.add(new SwerveCalculatorNode(timeForHalf, -acceleration, goalVelocity));
-            } else {
-                // Going to peak will not work as it will exceed our maximumVelocity limit
-                // Go to max, then cruise, then decelerate
-                double timeFromGoalToMaxVelocity = (maxVelocity - currentVelocity) / acceleration;
-                double initialPosition = (endPosition - startPosition) - leftoverDistance;
-                double finalPositionAfterAcceleration = 0.5 * (currentVelocity + maxVelocity)
-                        * timeFromGoalToMaxVelocity + initialPosition;
-                double positionDelta = finalPositionAfterAcceleration - initialPosition;
-                double cruiseDistance = leftoverDistance - (positionDelta * 2);
-                double cruiseTime = cruiseDistance / maxVelocity;
-
-                nodeMap.add(new SwerveCalculatorNode(timeFromGoalToMaxVelocity, acceleration, maxVelocity));
-                nodeMap.add(new SwerveCalculatorNode(cruiseTime, 0, maxVelocity));
-                nodeMap.add(new SwerveCalculatorNode(timeFromGoalToMaxVelocity, -acceleration, goalVelocity));
-            }
-            return nodeMap;
-
-        } else {
-            // Cruise til the end if we are at both goal & max velocity!
-            double cruiseTime = leftoverDistance / currentVelocity;
-            nodeMap.add(new SwerveCalculatorNode(
-                    cruiseTime,
-                    0,
-                    currentVelocity
-            ));
-            return nodeMap;
-        }
-    }
-
-    public List<SwerveCalculatorNode> getNodeMap() {
-        return nodeMap;
-    }
-
-    public double getVelocityAtFinish() {
-        SwerveCalculatorNode finalNode = nodeMap.get(nodeMap.size() - 1);
-        return finalNode.operationEndingVelocity();
-    }
-
-    public double getTotalOperationTime() {
-        double time = 0;
-        for (SwerveCalculatorNode node : this.nodeMap) {
-            time += node.operationTime();
-        }
-        return time;
-    }
-
-    public double getDistanceTravelledInMetersAtTime(double time) {
-        if (time < 0) {
-            assertionManager.throwException("Invalid time to getDistanceTravelledInMetersAtTime", new Exception());
-            return 0;
-        }
-        double elapsedTime = 0;
-        double totalDistance = 0;
-        double velocity = initialVelocity;
-        for (SwerveCalculatorNode node : this.nodeMap) {
-            // Get amount of time elapsed (no exceed time)
-            // Add distance with node
-            double operationTime = node.operationTime();
-            if ((time - (operationTime + elapsedTime)) >= 0) {
-                double distanceTravelled = velocity * operationTime
-                        + 0.5 * node.operationAcceleration() * Math.pow(operationTime, 2);
-
-                totalDistance += distanceTravelled;
-                velocity = node.operationEndingVelocity();
-                elapsedTime += node.operationTime();
-            } else {
-                // Find the amount of time we'll be using the node
-                operationTime = time - elapsedTime;
-                double distanceTravelled = velocity * operationTime
-                        + 0.5 * node.operationAcceleration() * Math.pow(operationTime, 2);
-                totalDistance += distanceTravelled;
-                break;
-            }
-        }
-        return totalDistance;
-    }
-
-    // Range: 0 - 1 (not in actual percentages!!!)
-    public double geDistanceTravelledAtCompletionPercentage(double percentage) {
-        double time = getTotalOperationTime() * percentage;
-        return getDistanceTravelledInMetersAtTime(time);
-    }
-
-    public double getTotalDistanceInMeters() {
-        return Math.abs(endPosition - startPosition);
-    }
-
-    public double getVelocityAtDistanceTravelled(double distanceTravelled) {
-        double totalDistanceTravelled = 0;
-        double currentVelocity = initialVelocity;
-        for (SwerveCalculatorNode node : this.nodeMap) {
-
-            // Find amount of distance current node travels
-            double operationDistance = (currentVelocity * node.operationTime()) + (0.5
-                    * node.operationAcceleration()) * Math.pow(node.operationTime(), 2);
-
-            // Continue until we land on the node we stop in
-            if (distanceTravelled - (totalDistanceTravelled + operationDistance) >= 0) {
-                totalDistanceTravelled += operationDistance;
-                currentVelocity += (node.operationAcceleration() * node.operationTime());
-            } else {
-                // Accelerate the remaining distance
-                double operationTime = calculateTimeToGoalPosition(
-                        node.operationAcceleration(),
-                        currentVelocity,
-                        totalDistanceTravelled,
-                        distanceTravelled);
-                currentVelocity += (node.operationAcceleration() * operationTime);
-                break;
-            }
-        }
-        return currentVelocity;
-    }
-}
diff --git a/src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java b/src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java
deleted file mode 100644
index 38ea27d1e..000000000
--- a/src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java
+++ /dev/null
@@ -1,28 +0,0 @@
-package xbot.common.subsystems.drive;
-
-// Contains a set of kinematics values
-public class SwervePointKinematics {
-    final double acceleration; //  Units: m/s^2
-    final double initialVelocity; // Units: m/s
-    final double goalVelocity; // m/s, velocity you want to be when you reach your goal, may not always be fulfilled.
-    final double maxVelocity; // m/s
-
-    public SwervePointKinematics(double a, double initialVelocity, double goalVelocity, double maxVelocity) {
-        this.acceleration = a;
-        this.initialVelocity = initialVelocity;
-        this.goalVelocity = Math.max(Math.min(maxVelocity, goalVelocity), -maxVelocity);
-        this.maxVelocity = maxVelocity;
-    }
-
-    public SwervePointKinematics kinematicsWithNewVi(double newVi) {
-        return new SwervePointKinematics(acceleration, newVi, goalVelocity, maxVelocity);
-    }
-
-    public double getAcceleration() {
-        return this.acceleration;
-    }
-
-    public double getMaxVelocity() {
-        return this.maxVelocity;
-    }
-}
diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java
index 804a748da..6663d3474 100644
--- a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java
+++ b/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.geometry.Twist2d;
 import xbot.common.command.BaseCommand;
-import xbot.common.logging.RobotAssertionManager;
 import xbot.common.math.XYPair;
 import xbot.common.properties.PropertyFactory;
 import xbot.common.subsystems.drive.control_logic.HeadingModule;
@@ -25,14 +24,14 @@ public class SwerveSimpleTrajectoryCommand extends BaseCommand {
 
     @Inject
     public SwerveSimpleTrajectoryCommand(BaseSwerveDriveSubsystem drive, BasePoseSubsystem pose, PropertyFactory pf,
-                                         HeadingModuleFactory headingModuleFactory, RobotAssertionManager assertionManager) {
+                                         HeadingModuleFactory headingModuleFactory) {
         this.drive = drive;
         this.pose = pose;
         headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());
 
         pf.setPrefix(this);
         this.addRequirements(drive);
-        logic = new SwerveSimpleTrajectoryLogic(assertionManager);
+        logic = new SwerveSimpleTrajectoryLogic();
         alternativeIsFinishedSupplier = () -> false;
     }
 
diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java b/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java
deleted file mode 100644
index d88553a3b..000000000
--- a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java
+++ /dev/null
@@ -1,9 +0,0 @@
-package xbot.common.subsystems.drive;
-
-/* Different ways to render the SwerveSimpleTrajectoryCommand's path */
-public enum SwerveSimpleTrajectoryMode {
-    ConstantVelocity, // We'll use a constant velocity throughout the course of our travel
-    DurationInSeconds, // Each point will be given a "duration"
-    KinematicsForIndividualPoints, // Each individual point will have its own kinematics values it'll go at
-    KinematicsForPointsList // One kinematics values for the entire route
-}
diff --git a/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java b/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java
index 171737dfb..d9bb24a29 100644
--- a/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java
+++ b/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java
@@ -333,7 +333,6 @@ public void periodic() {
             isNavXReady = true;
         }   
         updatePose();
-        aKitLog.record("Heading", currentHeading.getDegrees());
     }
 
     @Override
diff --git a/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java b/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java
index 666eba99d..156d00621 100644
--- a/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java
+++ b/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import xbot.common.subsystems.drive.SwervePointKinematics;
 
 public interface ProvidesInterpolationData {
     public Translation2d getTranslation2d();
@@ -10,6 +9,4 @@ public interface ProvidesInterpolationData {
     public double getSecondsForSegment();
 
     public Rotation2d getRotation2d();
-
-    public SwervePointKinematics getKinematics();
 }
\ No newline at end of file
diff --git a/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java b/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java
index e8b8c2875..bd98b522c 100644
--- a/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java
+++ b/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java
@@ -7,10 +7,6 @@
 
 import xbot.common.advantage.AKitLogger;
 import xbot.common.controls.sensors.XTimer;
-import xbot.common.logging.RobotAssertionManager;
-import xbot.common.subsystems.drive.SwerveKinematicsCalculator;
-import xbot.common.subsystems.drive.SwervePointKinematics;
-import xbot.common.subsystems.drive.SwerveSimpleTrajectoryMode;
 
 import java.util.List;
 public class SimpleTimeInterpolator {
@@ -21,13 +17,10 @@ public class SimpleTimeInterpolator {
     int index;
     double maximumDistanceFromChasePointInMeters = 0.3;
 
-    SwerveKinematicsCalculator calculator;
-
     private List<? extends ProvidesInterpolationData> keyPoints;
 
     Logger log = LogManager.getLogger(SimpleTimeInterpolator.class);
     AKitLogger aKitLog = new AKitLogger("SimpleTimeInterpolator/");
-    RobotAssertionManager assertionManager;
 
     public class InterpolationResult {
         public Translation2d chasePoint;
@@ -78,9 +71,7 @@ public InterpolationResult(Translation2d chasePoint, boolean isOnFinalPoint, Rot
         }
     }
 
-    public SimpleTimeInterpolator(RobotAssertionManager assertionManager) {
-        this.assertionManager = assertionManager;
-    }
+    public SimpleTimeInterpolator() {}
 
     public void setKeyPoints(List<? extends ProvidesInterpolationData > keyPoints) {
         this.keyPoints = keyPoints;
@@ -96,23 +87,9 @@ public void initialize(ProvidesInterpolationData baseline) {
         accumulatedProductiveSeconds = 0;
         previousTimestamp = XTimer.getFPGATimestamp();
         index = 0;
-        aKitLog.record("freeze proc", false);
-    }
-
-    public SwerveKinematicsCalculator newCalculator(Translation2d targetPointTranslation2d, SwervePointKinematics kinematics) {
-        return new SwerveKinematicsCalculator(
-                assertionManager,
-                0,
-                baseline.getTranslation2d().minus(targetPointTranslation2d).getNorm(),
-                kinematics
-        );
     }
 
-    public void resetCalculator() {
-        calculator = null;
-    }
-
-    public InterpolationResult calculateTarget(Translation2d currentLocation, SwerveSimpleTrajectoryMode mode) {
+    public InterpolationResult calculateTarget(Translation2d currentLocation) {
         double currentTime = XTimer.getFPGATimestamp();
         aKitLog.record("CurrentTime", currentTime);
 
@@ -135,14 +112,6 @@ public InterpolationResult calculateTarget(Translation2d currentLocation, Swerve
             return new InterpolationResult(currentLocation, true, targetKeyPoint.getRotation2d());
         }
 
-        if ((mode == SwerveSimpleTrajectoryMode.KinematicsForIndividualPoints
-                || mode == SwerveSimpleTrajectoryMode.KinematicsForPointsList) && calculator == null) {
-            calculator = newCalculator(
-                    targetKeyPoint.getTranslation2d(),
-                    targetKeyPoint.getKinematics()
-            );
-        }
-
         // First, assume we are just going to our target. (This is what all trajectories will eventually
         // settle to - all this interpolation is for intermediate points.)
         Translation2d chasePoint = targetKeyPoint.getTranslation2d();
@@ -153,6 +122,8 @@ public InterpolationResult calculateTarget(Translation2d currentLocation, Swerve
         aKitLog.record("accumulatedProductiveSeconds", accumulatedProductiveSeconds);
 
 
+
+
         // If the fraction is above 1, it's time to set a new baseline point and start LERPing on the next
         // one.
         if (lerpFraction >= 1 && index < keyPoints.size()-1) {
@@ -166,34 +137,19 @@ public InterpolationResult calculateTarget(Translation2d currentLocation, Swerve
             targetKeyPoint = keyPoints.get(index);
             log.info("Baseline is now " + baseline.getTranslation2d()
                     + " and target is now " + targetKeyPoint.getTranslation2d());
-
-            if (mode == SwerveSimpleTrajectoryMode.KinematicsForIndividualPoints || mode == SwerveSimpleTrajectoryMode.KinematicsForPointsList) {
-                calculator = newCalculator(
-                        targetKeyPoint.getTranslation2d(),
-                        targetKeyPoint.getKinematics()
-                );
-            }
         }
 
         // Most of the time, the fraction will be less than one.
         // In that case, we want to interpolate between the baseline and the target.
-        double multiplier = 1;
         if (lerpFraction < 1) {
-            if (mode == SwerveSimpleTrajectoryMode.KinematicsForIndividualPoints || mode == SwerveSimpleTrajectoryMode.KinematicsForPointsList) {
-                double magnitude = calculator.geDistanceTravelledAtCompletionPercentage(lerpFraction);
-                multiplier = magnitude / calculator.getTotalDistanceInMeters(); // Magnitude progress
-                chasePoint = baseline.getTranslation2d().interpolate(
-                        targetKeyPoint.getTranslation2d(), multiplier);
-            } else {
-                chasePoint = baseline.getTranslation2d().interpolate(
-                        targetKeyPoint.getTranslation2d(), lerpFraction);
-            }
+            chasePoint = baseline.getTranslation2d().interpolate(
+                    targetKeyPoint.getTranslation2d(), lerpFraction);
         }
 
         // But if that chase point is "too far ahead", we need to freeze the chasePoint
+        // until the robot has a chance to catch up.
         if (currentLocation.getDistance(chasePoint) > maximumDistanceFromChasePointInMeters) {
             // This effectively "rewinds time" for the next loop.
-            aKitLog.record("Freezing ChasePoint", true);
             accumulatedProductiveSeconds -= secondsSinceLastExecute;
         }
 
@@ -201,11 +157,11 @@ public InterpolationResult calculateTarget(Translation2d currentLocation, Swerve
         var plannedVector = targetKeyPoint.getTranslation2d().minus(baseline.getTranslation2d())
                 .div(targetKeyPoint.getSecondsForSegment());
 
-        plannedVector.times(multiplier > 0 ? multiplier : 0.01);
-
         boolean targetingFinalPoint = index == keyPoints.size()-1 && lerpFraction >= 1;
         boolean isOnFinalLeg = index == keyPoints.size()-1;
         return new InterpolationResult(chasePoint, targetingFinalPoint, targetKeyPoint.getRotation2d(), plannedVector,
                 currentLocation.getDistance(targetKeyPoint.getTranslation2d()), lerpFraction, isOnFinalLeg);
     }
+
+
 }
\ No newline at end of file
diff --git a/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java b/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java
index b0deef408..559c07cce 100644
--- a/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java
+++ b/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java
@@ -5,14 +5,10 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import org.apache.logging.log4j.LogManager;
-import org.apache.logging.log4j.Logger;
+
 import xbot.common.advantage.AKitLogger;
-import xbot.common.logging.RobotAssertionManager;
 import xbot.common.math.PIDManager;
 import xbot.common.math.XYPair;
-import xbot.common.subsystems.drive.SwerveKinematicsCalculator;
-import xbot.common.subsystems.drive.SwervePointKinematics;
-import xbot.common.subsystems.drive.SwerveSimpleTrajectoryMode;
 import xbot.common.subsystems.drive.control_logic.HeadingModule;
 
 import java.util.ArrayList;
@@ -21,13 +17,15 @@
 
 public class SwerveSimpleTrajectoryLogic {
 
-    Logger log = LogManager.getLogger(this.getClass());
+    org.apache.logging.log4j.Logger log = LogManager.getLogger(this.getClass());
     final AKitLogger aKitLog = new AKitLogger("SimpleTimeInterpolator/");
 
     private Supplier<List<XbotSwervePoint>> keyPointsProvider;
     private List<XbotSwervePoint> keyPoints;
+    private boolean enableConstantVelocity = false;
+    private double constantVelocity = 0.25;
     private boolean stopWhenFinished = true;
-    private final SimpleTimeInterpolator interpolator;
+    private final SimpleTimeInterpolator interpolator = new SimpleTimeInterpolator();
     SimpleTimeInterpolator.InterpolationResult lastResult;
     double maxPower = 1.0;
     double maxTurningPower = 1.0;
@@ -40,14 +38,9 @@ public class SwerveSimpleTrajectoryLogic {
     private Pose2d specialAimTarget;
     private boolean prioritizeRotationIfCloseToGoal = false;
     private double distanceThresholdToPrioritizeRotation = 1.5;
-    private double constantVelocity = 0;
-    private SwervePointKinematics globalKinematics = null;
-    private SwerveSimpleTrajectoryMode mode = SwerveSimpleTrajectoryMode.DurationInSeconds;
-    RobotAssertionManager assertionManager;
-
-    public SwerveSimpleTrajectoryLogic(RobotAssertionManager assertionManager) {
-        this.assertionManager = assertionManager;
-        interpolator = new SimpleTimeInterpolator(this.assertionManager);
+
+    public SwerveSimpleTrajectoryLogic() {
+
     }
 
     // --------------------------------------------------------------
@@ -98,10 +91,16 @@ public void setMaxTurningPower(double maxTurningPower) {
         this.maxTurningPower = maxTurningPower;
     }
 
-    public List<XbotSwervePoint> getKeyPoints() {
-        return this.keyPointsProvider.get();
+    public void setEnableConstantVelocity(boolean enableConstantVelocity) {
+        this.enableConstantVelocity = enableConstantVelocity;
+    }
+
+    public void setConstantVelocity(double constantVelocity) {
+        this.constantVelocity = constantVelocity;
     }
 
+    public List<XbotSwervePoint> getKeyPoints() { return this.keyPointsProvider.get(); }
+
     public List<XbotSwervePoint> getResolvedKeyPoints() {
         return keyPoints;
     }
@@ -118,18 +117,6 @@ public void setDistanceThresholdToPrioritizeRotation(double distanceThresholdToP
         this.distanceThresholdToPrioritizeRotation = distanceThresholdToPrioritizeRotation;
     }
 
-    public void setVelocityMode(SwerveSimpleTrajectoryMode mode) {
-        this.mode = mode;
-    }
-
-    public void setConstantVelocity(double constantVelocity) {
-        this.constantVelocity = constantVelocity;
-    }
-
-    public void setGlobalKinematicValues(SwervePointKinematics globalKinematics) {
-        this.globalKinematics = globalKinematics;
-    }
-
     // --------------------------------------------------------------
     // Major Command Elements
     // --------------------------------------------------------------
@@ -138,7 +125,6 @@ public void reset(Pose2d currentPose) {
         log.info("Resetting");
         keyPoints = keyPointsProvider.get();
         log.info("Key points size: " + keyPoints.size());
-        interpolator.resetCalculator();
 
         var initialPoint = new XbotSwervePoint(currentPose, 0);
 
@@ -149,7 +135,7 @@ public void reset(Pose2d currentPose) {
             // Visualize direct raycast
             var start = new XbotSwervePoint(currentPose.getTranslation(), currentPose.getRotation(), 0);
             var raycast = XbotSwervePoint.generateTrajectory(List.of(
-                    start, keyPoints.get(0))
+                start, keyPoints.get(0))
             );
             aKitLog.record("Raycast", raycast);
 
@@ -158,28 +144,8 @@ public void reset(Pose2d currentPose) {
                     new Pose2d(targetPoint.getTranslation2d(), targetPoint.getRotation2d()));
         }
 
-        // Adjust points based on mode, since the logic at default is optimized for "DurationInSeconds" we need
-        // To convert over modes to have values in DurationInSeconds.
-        switch (mode) {
-            case DurationInSeconds -> {} // The logic at default is optimized for duration in seconds.
-            case KinematicsForIndividualPoints -> {
-                for (XbotSwervePoint point : keyPoints) {
-                    if (point.getKinematics() == null) {
-                        assertionManager.throwException("Needs to set kinematics for swerve point!", new Exception());
-                    }
-                }
-                keyPoints = getKinematicsAdjustedSwervePoints(initialPoint, keyPoints);
-            }
-            case KinematicsForPointsList -> {
-                if (globalKinematics == null) {
-                    assertionManager.throwException("Needs to set globalKinematics!", new Exception());
-                }
-                keyPoints = getGlobalKinematicsAdjustedSwervePoints(initialPoint, keyPoints, currentPose);
-            }
-            case ConstantVelocity -> {
-                keyPoints = getVelocityAdjustedSwervePoints(initialPoint, keyPoints, constantVelocity);
-            }
-            default -> assertionManager.throwException("No handling for SwerveSimpleTrajectoryMode!", new Exception());
+        if (enableConstantVelocity) {
+            keyPoints = getVelocityAdjustedSwervePoints(initialPoint, keyPoints, constantVelocity);
         }
 
         handleAimingAtFinalLeg(currentPose);
@@ -212,7 +178,8 @@ public void reset(Pose2d currentPose) {
             }
         }
 
-        aKitLog.record("Trajectory", XbotSwervePoint.generateTrajectory(keyPoints));
+        aKitLog.record("Trajectory",
+                XbotSwervePoint.generateTrajectory(keyPoints));
 
         interpolator.setMaximumDistanceFromChasePointInMeters(0.5);
         interpolator.setKeyPoints(keyPoints);
@@ -296,111 +263,8 @@ private List<XbotSwervePoint> getVelocityAdjustedSwervePoints(
         return velocityAdjustedPoints;
     }
 
-    private List<XbotSwervePoint> getKinematicsAdjustedSwervePoints(
-            XbotSwervePoint initialPoint,
-            List<XbotSwervePoint> swervePoints) {
-
-        ArrayList<XbotSwervePoint> adjustedPoints = new ArrayList<>();
-
-        SwerveKinematicsCalculator calculator = null;
-        for (int i = 0; i < swervePoints.size(); i++) {
-            XbotSwervePoint previous = initialPoint;
-            var current = swervePoints.get(i);
-            if (i > 0) {
-                // If we've moved on to later points, we can now safely get previous entries in the list.
-                previous = swervePoints.get(i - 1);
-                // Calculate the initial velocity of current node
-                current.setKinematics(current.kinematics.kinematicsWithNewVi(calculator.getVelocityAtFinish()));
-            }
-            double distance = previous.getTranslation2d().getDistance(current.getTranslation2d());
-            calculator = new SwerveKinematicsCalculator(
-                    assertionManager,
-                    0,
-                    distance,
-                    current.getKinematics()
-            );
-            double adjustedDuration = calculator.getTotalOperationTime();
-
-            if (adjustedDuration > 0) {
-                XbotSwervePoint point = new XbotSwervePoint(current.keyPose, adjustedDuration);
-                point.setKinematics(current.getKinematics());
-                adjustedPoints.add(point);
-            }
-        }
-
-        if (adjustedPoints.size() == 0) {
-            var dummyPoint = new XbotSwervePoint(initialPoint.keyPose, 0.05);
-            adjustedPoints.add(dummyPoint);
-        }
-
-        return adjustedPoints;
-    }
-
-    // Instead of using kinematic values of each individual SwervePoint
-    // We use kinematic values "globalKinematics" set in this logic
-    private List<XbotSwervePoint> getGlobalKinematicsAdjustedSwervePoints(
-            XbotSwervePoint initialPoint,
-            List<XbotSwervePoint> swervePoints,
-            Pose2d startingPose) {
-
-        ArrayList<XbotSwervePoint> adjustedPoints = new ArrayList<>();
-
-        double totalDistance = 0;
-        Translation2d currentPosition = startingPose.getTranslation();
-        for (XbotSwervePoint point : swervePoints) {
-            totalDistance += currentPosition.getDistance(point.keyPose.getTranslation());
-            currentPosition = point.keyPose.getTranslation();
-        }
-
-        SwerveKinematicsCalculator calculator = new SwerveKinematicsCalculator(
-                assertionManager,
-                0,
-                totalDistance,
-                globalKinematics
-        );
-
-        double accumulatedDistance = 0;
-        for (int i = 0; i < swervePoints.size(); i++) {
-            XbotSwervePoint previous = initialPoint;
-            var current = swervePoints.get(i);
-            if (i > 0) {
-                // If we've moved on to later points, we can now safely get previous entries in the list.
-                previous = swervePoints.get(i - 1);
-            }
-
-            // NEED: acceleration, initialVelocity, finalVelocity, maxVelocity,
-            // we got a and vMax which is global now we need vInitial and vFinal
-            double vi = calculator.getVelocityAtDistanceTravelled(accumulatedDistance);
-            double distance = previous.getTranslation2d().getDistance(current.getTranslation2d());
-            accumulatedDistance += distance;
-            double vf = calculator.getVelocityAtDistanceTravelled(accumulatedDistance);
-
-            SwerveKinematicsCalculator operationCalculator = new SwerveKinematicsCalculator(
-                    assertionManager,
-                    0,
-                    distance,
-                    new SwervePointKinematics(globalKinematics.getAcceleration(), vi, vf, globalKinematics.getMaxVelocity())
-            );
-
-            double adjustedDuration = operationCalculator.getTotalOperationTime();
-
-            if (adjustedDuration > 0) {
-                XbotSwervePoint point = new XbotSwervePoint(current.keyPose, adjustedDuration);
-                point.setKinematics(new SwervePointKinematics(globalKinematics.getAcceleration(), vi, vf, globalKinematics.getMaxVelocity()));
-                adjustedPoints.add(point);
-            }
-        }
-
-        if (adjustedPoints.size() == 0) {
-            var dummyPoint = new XbotSwervePoint(initialPoint.keyPose, 0.05);
-            adjustedPoints.add(dummyPoint);
-        }
-
-        return adjustedPoints;
-    }
-
     public XYPair getGoalVector(Pose2d currentPose) {
-        lastResult = interpolator.calculateTarget(currentPose.getTranslation(), mode);
+        lastResult = interpolator.calculateTarget(currentPose.getTranslation());
         var chasePoint = lastResult.chasePoint;
 
         aKitLog.record("chasePoint", new Pose2d(chasePoint, Rotation2d.fromDegrees(0)));
@@ -442,7 +306,7 @@ public Twist2d calculatePowers(Pose2d currentPose, PIDManager positionalPid, Hea
 
         boolean lastLegAndSpecialAim = enableSpecialAimDuringFinalLeg && lastResult.isOnFinalLeg;
 
-        if (specialAimTarget != null && (enableSpecialAimTarget || lastLegAndSpecialAim)) {
+        if (specialAimTarget != null && (enableSpecialAimTarget || lastLegAndSpecialAim )) {
             degreeTarget = getAngleBetweenTwoPoints(
                     currentPose.getTranslation(), specialAimTarget.getTranslation()
             ).getDegrees();
@@ -460,13 +324,12 @@ public Twist2d calculatePowers(Pose2d currentPose, PIDManager positionalPid, Hea
         }
 
         aKitLog.record("UpdatedIntent", intent);
-
-        // In the future we can add different adjustments for if we are ahead/behind in schedule
         // If we have no max velocity set, or we are on the last point and almost done, just use the position PID
         if (maximumVelocity <= 0 || (lastResult.isOnFinalPoint && lastResult.distanceToTargetPoint < 0.5) || lastResult.lerpFraction > 1) {
             return new Twist2d(intent.x, intent.y, headingPower);
-
-        } else {
+        }
+        else
+        {
             // Otherwise, add the known velocity vector, so we can catch up to our goal.
 
             // Get the dot product between the normalized goal vector and the normalized velocity vector.
@@ -530,4 +393,6 @@ public boolean recommendIsFinished(Pose2d currentPose, PIDManager positionalPid,
     public SimpleTimeInterpolator.InterpolationResult getLastResult() {
         return lastResult;
     }
+
+
 }
diff --git a/src/main/java/xbot/common/trajectory/XbotSwervePoint.java b/src/main/java/xbot/common/trajectory/XbotSwervePoint.java
index 2112f75d0..681462f61 100644
--- a/src/main/java/xbot/common/trajectory/XbotSwervePoint.java
+++ b/src/main/java/xbot/common/trajectory/XbotSwervePoint.java
@@ -5,7 +5,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.Trajectory;
 import xbot.common.math.WrappedRotation2d;
-import xbot.common.subsystems.drive.SwervePointKinematics;
 import xbot.common.subsystems.pose.BasePoseSubsystem;
 
 import java.util.ArrayList;
@@ -16,7 +15,6 @@ public class XbotSwervePoint implements ProvidesInterpolationData {
     public Pose2d keyPose;
 
     public double secondsToPoint;
-    SwervePointKinematics kinematics;
 
     public XbotSwervePoint(Pose2d keyPose, double secondsToPoint) {
         this.keyPose = keyPose;
@@ -33,13 +31,6 @@ public XbotSwervePoint(double x, double y, double degrees, double secondsToPoint
         this.secondsToPoint = secondsToPoint;
     }
 
-    public void setKinematics(SwervePointKinematics kinematics) {
-        this.kinematics = kinematics;
-    }
-
-    public SwervePointKinematics getKinematics() {
-        return kinematics;
-    }
 
     public void setPose(Pose2d pose) {
         this.keyPose = pose;
diff --git a/src/test/java/xbot/common/subsystems/drive/SwerveCalculatorTest.java b/src/test/java/xbot/common/subsystems/drive/SwerveCalculatorTest.java
deleted file mode 100644
index d8114335f..000000000
--- a/src/test/java/xbot/common/subsystems/drive/SwerveCalculatorTest.java
+++ /dev/null
@@ -1,125 +0,0 @@
-package xbot.common.subsystems.drive;
-
-import org.junit.Test;
-import xbot.common.injection.BaseCommonLibTest;
-import xbot.common.logging.RobotAssertionManager;
-
-import javax.inject.Inject;
-import java.util.ArrayList;
-import java.util.List;
-
-import static org.junit.Assert.assertEquals;
-
-public class SwerveCalculatorTest extends BaseCommonLibTest {
-
-    RobotAssertionManager assertionManager;
-
-    @Override
-    public void setUp() {
-        super.setUp();
-        this.assertionManager = getInjectorComponent().robotAssertionManager();
-    }
-
-    // startPosition, endPosition, maximumAcceleration, startingVelocity, goalVelocity, maximumVelocity
-    private SwerveKinematicsCalculator newCalculator(double a, double vInitial, double vGoal, double vMax) {
-        return new SwerveKinematicsCalculator(
-                assertionManager,
-                0,
-                10,
-                new SwervePointKinematics(a, vInitial, vGoal, vMax)
-        );
-    }
-
-    private void compareNodeMaps(List<SwerveCalculatorNode> map1, List<SwerveCalculatorNode> map2) {
-        assertEquals(map1.size(), map2.size());
-        for (int i = 0; i < map1.size(); i++) {
-            assertEquals(map1.get(i).operationTime(), map2.get(i).operationTime(), 0.001);
-            assertEquals(map1.get(i).operationAcceleration(), map2.get(i).operationAcceleration(), 0.001);
-            assertEquals(map1.get(i).operationEndingVelocity(), map2.get(i).operationEndingVelocity(), 0.001);
-        }
-    }
-
-    @Test
-    public void testAccelerateMaxThenCruiseThenGoal() {
-        // Accelerate to max, then cruise, then down to goal
-        SwerveKinematicsCalculator calculator = newCalculator(0.5, 0, 0, 2);
-        List<SwerveCalculatorNode> nodeMap = new ArrayList<>();
-        nodeMap.add(new SwerveCalculatorNode(4, 0.5, 2));
-        nodeMap.add(new SwerveCalculatorNode(1, 0, 2));
-        nodeMap.add(new SwerveCalculatorNode(4, -0.5, 0));
-        compareNodeMaps(nodeMap, calculator.getNodeMap());
-    }
-
-    @Test
-    public void testAccelerateGoalThenMaxThenCruiseThenGoal() {
-        // Accelerate to goal, then max, then cruise, then down to goal
-        SwerveKinematicsCalculator calculator = newCalculator(0.5, 0., 1, 2);
-        List<SwerveCalculatorNode> nodeMap = new ArrayList<>();
-        nodeMap.add(new SwerveCalculatorNode(2, 0.5, 1));
-        nodeMap.add(new SwerveCalculatorNode(2, 0.5, 2));
-        nodeMap.add(new SwerveCalculatorNode(1.5, 0, 2));
-        nodeMap.add(new SwerveCalculatorNode(2, -0.5, 1));
-        compareNodeMaps(nodeMap, calculator.getNodeMap());
-    }
-
-    @Test
-    public void testAccelerateGoalThenAccelerateThenDecelerate() {
-        // Accelerate to goal, then accelerate, then decelerate, forming a peak, will not reach vMax so no cruise
-        SwerveKinematicsCalculator calculator = newCalculator(0.5, 0, 1, 5);
-        List<SwerveCalculatorNode> nodeMap = new ArrayList<>();
-        nodeMap.add(new SwerveCalculatorNode(2, 0.5, 1));
-        nodeMap.add(new SwerveCalculatorNode(2.69042, 0.5, 2.34521));
-        nodeMap.add(new SwerveCalculatorNode(2.69042, -0.5, 1));
-        compareNodeMaps(nodeMap, calculator.getNodeMap());
-
-    }
-
-    @Test
-    public void testAccelerateAsMuchAsPossible() {
-        // Accelerate as much as possible because endingVelocity is impossible to reach
-        SwerveKinematicsCalculator calculator = newCalculator(0.5, 0, 10, 10);
-        List<SwerveCalculatorNode> nodeMap = new ArrayList<>();
-        nodeMap.add(new SwerveCalculatorNode(6.32456, 0.5, 3.16228));
-        compareNodeMaps(nodeMap, calculator.getNodeMap());
-    }
-
-    @Test
-    public void testAccelerateThenCruiseToTheEnd() {
-        // Accelerate to goal velocity, in this case will be the same as max velocity, then cruise
-        SwerveKinematicsCalculator calculator = newCalculator(0.5, 0, 3, 3);
-        List<SwerveCalculatorNode> nodeMap = new ArrayList<>();
-        nodeMap.add(new SwerveCalculatorNode(6, 0.5, 3));
-        nodeMap.add(new SwerveCalculatorNode(0.33333, 0, 3));
-        compareNodeMaps(nodeMap, calculator.getNodeMap());
-    }
-
-    @Test
-    public void testDecelerateAsMuchAsPossible() {
-        // Decelerate as much as possible because endingVelocity is impossible to reach
-        SwerveKinematicsCalculator calculator = newCalculator(0.5, 8, 0, 10);
-        List<SwerveCalculatorNode> nodeMap = new ArrayList<>();
-        nodeMap.add(new SwerveCalculatorNode(1.30306, -0.5, 7.34847));
-        compareNodeMaps(nodeMap, calculator.getNodeMap());
-    }
-
-    @Test
-    public void testAccelerateThenCruiseThenDecelerateToGoal() {
-        SwerveKinematicsCalculator calculator = newCalculator(0.5, 1, 0, 1.5);
-        List<SwerveCalculatorNode> nodeMap = new ArrayList<>();
-        nodeMap.add(new SwerveCalculatorNode(1, 0.5, 1.5));
-        nodeMap.add(new SwerveCalculatorNode(4.33333, 0, 1.5));
-        nodeMap.add(new SwerveCalculatorNode(1, -0.5, 1));
-        nodeMap.add(new SwerveCalculatorNode(2, -0.5, 0));
-        compareNodeMaps(nodeMap, calculator.getNodeMap());
-    }
-
-    @Test
-    public void testAccelerateThenDecelerateToGoal() {
-        SwerveKinematicsCalculator calculator = newCalculator(0.5, 1, 0, 10);
-        List<SwerveCalculatorNode> nodeMap = new ArrayList<>();
-        nodeMap.add(new SwerveCalculatorNode(2.69042, 0.5, 2.34521));
-        nodeMap.add(new SwerveCalculatorNode(2.69042, -0.5, 1));
-        nodeMap.add(new SwerveCalculatorNode(2, -0.5, 0));
-        compareNodeMaps(nodeMap, calculator.getNodeMap());
-    }
-}