Removed Vector2d in favor of using Translation2d polar to cartesian…

… conversion.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -36,7 +36,6 @@
 import swervelib.SwerveDrive;
 import swervelib.SwerveDriveTest;
 import swervelib.math.SwerveMath;
-import swervelib.math.Vector2d;
 import swervelib.parser.SwerveControllerConfiguration;
 import swervelib.parser.SwerveDriveConfiguration;
 import swervelib.parser.SwerveParser;
@@ -49,7 +48,7 @@ public class SwerveSubsystem extends SubsystemBase
   /**
    * Swerve drive object.
    */
-  private final SwerveDrive swerveDrive;
+  private final SwerveDrive         swerveDrive;
   /**
    * AprilTag field layout.
    */
@@ -252,8 +251,9 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat
   {
     // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control.
     return run(() -> {
-      Translation2d scaledInputs = new Vector2d(translationX.getAsDouble(), translationY.getAsDouble()).pow(3)
-                                                                                                       .getTranslation();
+
+      Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(translationX.getAsDouble(),
+                                                                                translationY.getAsDouble()));
 
       // Make the robot move
       driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(),
@@ -325,9 +325,9 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat
   {
     return run(() -> {
       // Make the robot move
-      swerveDrive.drive(new Vector2d(new Translation2d(translationX.getAsDouble() * swerveDrive.getMaximumVelocity(),
-                                                       translationY.getAsDouble() *
-                                                       swerveDrive.getMaximumVelocity())).pow(3).getTranslation(),
+      swerveDrive.drive(SwerveMath.cubeTranslation(new Translation2d(
+                            translationX.getAsDouble() * swerveDrive.getMaximumVelocity(),
+                            translationY.getAsDouble() * swerveDrive.getMaximumVelocity())),
                         Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(),
                         true,
                         false);
@@ -508,7 +508,7 @@ public Rotation2d getHeading()
    */
   public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY)
   {
-    Translation2d scaledInputs = new Vector2d(xInput, yInput).pow(3).getTranslation();
+    Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
     return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
                                                         scaledInputs.getY(),
                                                         headingX,
@@ -528,7 +528,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin
    */
   public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle)
   {
-    Translation2d scaledInputs = new Vector2d(xInput, yInput).pow(3).getTranslation();
+    Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
 
     return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
                                                         scaledInputs.getY(),

src/main/java/swervelib/math/SwerveMath.java

@@ -412,14 +412,13 @@ public static void antiJitter(SwerveModuleState moduleState, SwerveModuleState l
   }
 
   /**
-   * Get the scaled {@link Translation2d} with the given scalar to change the magnitude of the {@link Vector2d}.
+   * Cube the {@link Translation2d} magnitude given in Polar coordinates.
    *
-   * @param cartesian {@link Translation2d} cartesian coordinates.
-   * @param scalar    Scalar to change the polar radius of the {@link Vector2d} by.
-   * @return {@link Translation2d} scaled by the given input.
+   * @param translation {@link Translation2d} to manipulate.
+   * @return Cubed magnitude from {@link Translation2d}.
    */
-  public static Translation2d scaleTranslation2d(Translation2d cartesian, double scalar)
+  public static Translation2d cubeTranslation(Translation2d translation)
   {
-    return new Vector2d(cartesian).scale(scalar).getTranslation();
+    return new Translation2d(Math.pow(translation.getNorm(), 3), translation.getAngle());
   }
 }