diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index a96c8112..f8bc912f 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/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(), diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java index dec51c1d..8506b5b7 100644 --- a/src/main/java/swervelib/math/SwerveMath.java +++ b/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()); } } diff --git a/src/main/java/swervelib/math/Vector2d.java b/src/main/java/swervelib/math/Vector2d.java deleted file mode 100644 index 13d33b76..00000000 --- a/src/main/java/swervelib/math/Vector2d.java +++ /dev/null @@ -1,96 +0,0 @@ -package swervelib.math; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** - * Vector containing Magnitude and Position - */ -public class Vector2d -{ - - /** - * Position given as an angle {@link Rotation2d}. - */ - public final Rotation2d position; - /** - * Arbitrary magnitude of the vector. - */ - public final double magnitude; - - /** - * Construct a Vector with a position and magnitude of 0. - */ - public Vector2d() - { - position = new Rotation2d(); - magnitude = 0; - } - - /** - * Create a vector based off of the given {@link Translation2d}. The magnitude is the - * {@link Math#hypot(double, double)} of the X and Y. - * - * @param cartesian {@link Translation2d} with the X and Y set. - */ - public Vector2d(Translation2d cartesian) - { - position = cartesian.getAngle(); - magnitude = cartesian.getNorm(); - } - - /** - * Construct a {@link Vector2d} given the position and magnitude. - * - * @param position Position as a {@link Rotation2d}. - * @param magnitude Magnitude as a double. - */ - public Vector2d(Rotation2d position, double magnitude) - { - this.position = position; - this.magnitude = magnitude; - } - - /** - * Convert cartesian coordinates to Vector. - * - * @param x X position - * @param y Y position - */ - public Vector2d(double x, double y) - { - this(new Translation2d(x, y)); - } - - /** - * Convert the Vector back into cartesian coordinates. - * - * @return {@link Translation2d} of the vector. - */ - public Translation2d getTranslation() - { - return new Translation2d(magnitude, position); - } - - /** - * Scale the magnitude by the multiplier given - * - * @param scalar Multiplier of the magnitude. - * @return {@link Vector2d} for chained functions. - */ - public Vector2d scale(double scalar) - { - return new Vector2d(position, magnitude * scalar); - } - - /** - * Exponentially modify the magnitude. - * - * @param pow Power for the magnitude - * @return {@link Vector2d} with the magnitude^pow - */ - public Vector2d pow(double pow) - { - return new Vector2d(position, Math.pow(this.magnitude, pow)); - } -}