Added scaling utilizing Vector2d which converts form cartesian coordinates to polar.

Signed-off-by: thenetworkgrinch <[email protected]>

Author: thenetworkgrinch

src/main/java/frc/robot/Constants.java

@@ -5,7 +5,6 @@
 package frc.robot;
 
 import com.pathplanner.lib.util.PIDConstants;
-
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import swervelib.math.Matter;
@@ -24,13 +23,14 @@ public final class Constants
   public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound
   public static final Matter CHASSIS    = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
   public static final double LOOP_TIME  = 0.13; //s, 20ms + 110ms sprk max velocity lag
-  public static final double MAX_SPEED = Units.feetToMeters(14.5); // Maximum speed of the robot in meters per second, used to limit acceleration.
+  public static final double MAX_SPEED  = Units.feetToMeters(14.5);
+      // Maximum speed of the robot in meters per second, used to limit acceleration.
 
   public static final class AutonConstants
   {
 
     public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0);
-    public static final PIDConstants ANGLE_PID   = new PIDConstants(0.4, 0, 0.01);
+    public static final PIDConstants ANGLE_PID       = new PIDConstants(0.4, 0, 0.01);
   }
 
   public static final class DrivebaseConstants

src/main/java/frc/robot/RobotContainer.java

@@ -27,13 +27,12 @@
 public class RobotContainer
 {
 
+  // Replace with CommandPS4Controller or CommandJoystick if needed
+  final CommandXboxController driverXbox = new CommandXboxController(0);
   // The robot's subsystems and commands are defined here...
   private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
                                                                          "swerve/neo"));
 
-  // Replace with CommandPS4Controller or CommandJoystick if needed
-  final CommandXboxController driverXbox = new CommandXboxController(0);
-
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
@@ -42,8 +41,6 @@ public RobotContainer()
     // Configure the trigger bindings
     configureBindings();
 
-
-
     // Applies deadbands and inverts controls because joysticks
     // are back-right positive while robot
     // controls are front-left positive
@@ -53,11 +50,11 @@ public RobotContainer()
     // WARNING: default buttons are on the same buttons as the ones defined in configureBindings
     AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase,
                                                                    () -> -MathUtil.applyDeadband(driverXbox.getLeftY(),
-                                                                                                OperatorConstants.LEFT_Y_DEADBAND),
+                                                                                                 OperatorConstants.LEFT_Y_DEADBAND),
                                                                    () -> -MathUtil.applyDeadband(driverXbox.getLeftX(),
-                                                                                                OperatorConstants.LEFT_X_DEADBAND),
+                                                                                                 OperatorConstants.LEFT_X_DEADBAND),
                                                                    () -> -MathUtil.applyDeadband(driverXbox.getRightX(),
-                                                                                                OperatorConstants.RIGHT_X_DEADBAND),
+                                                                                                 OperatorConstants.RIGHT_X_DEADBAND),
                                                                    driverXbox.getHID()::getYButtonPressed,
                                                                    driverXbox.getHID()::getAButtonPressed,
                                                                    driverXbox.getHID()::getXButtonPressed,

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

@@ -36,6 +36,7 @@
 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,6 +50,10 @@ public class SwerveSubsystem extends SubsystemBase
    * Swerve drive object.
    */
   private final SwerveDrive swerveDrive;
+  /**
+   * AprilTag field layout.
+   */
+  private final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
 
   /**
    * Initialize {@link SwerveDrive} with the directory provided.
@@ -131,11 +136,6 @@ public void setupPathPlanner()
                                   );
   }
 
-  /**
-   * AprilTag field layout.
-   */
-  private final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
-
   /**
    * Get the distance to the speaker.
    *
@@ -252,10 +252,11 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat
   {
     // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control.
     return run(() -> {
-      double xInput = Math.pow(translationX.getAsDouble(), 3); // Smooth controll out
-      double yInput = Math.pow(translationY.getAsDouble(), 3); // Smooth controll out
+      Translation2d scaledInputs = new Vector2d(translationX.getAsDouble(), translationY.getAsDouble()).pow(3)
+                                                                                                       .getTranslation();
+
       // Make the robot move
-      driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(xInput, yInput,
+      driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(),
                                                                       headingX.getAsDouble(),
                                                                       headingY.getAsDouble(),
                                                                       swerveDrive.getOdometryHeading().getRadians(),
@@ -324,8 +325,9 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat
   {
     return run(() -> {
       // Make the robot move
-      swerveDrive.drive(new Translation2d(Math.pow(translationX.getAsDouble(), 3) * swerveDrive.getMaximumVelocity(),
-                                          Math.pow(translationY.getAsDouble(), 3) * swerveDrive.getMaximumVelocity()),
+      swerveDrive.drive(new Vector2d(new Translation2d(translationX.getAsDouble() * swerveDrive.getMaximumVelocity(),
+                                                       translationY.getAsDouble() *
+                                                       swerveDrive.getMaximumVelocity())).pow(3).getTranslation(),
                         Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(),
                         true,
                         false);
@@ -506,10 +508,9 @@ public Rotation2d getHeading()
    */
   public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY)
   {
-    xInput = Math.pow(xInput, 3);
-    yInput = Math.pow(yInput, 3);
-    return swerveDrive.swerveController.getTargetSpeeds(xInput,
-                                                        yInput,
+    Translation2d scaledInputs = new Vector2d(xInput, yInput).pow(3).getTranslation();
+    return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
+                                                        scaledInputs.getY(),
                                                         headingX,
                                                         headingY,
                                                         getHeading().getRadians(),
@@ -527,10 +528,10 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin
    */
   public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle)
   {
-    xInput = Math.pow(xInput, 3);
-    yInput = Math.pow(yInput, 3);
-    return swerveDrive.swerveController.getTargetSpeeds(xInput,
-                                                        yInput,
+    Translation2d scaledInputs = new Vector2d(xInput, yInput).pow(3).getTranslation();
+
+    return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
+                                                        scaledInputs.getY(),
                                                         angle.getRadians(),
                                                         getHeading().getRadians(),
                                                         Constants.MAX_SPEED);

src/main/java/swervelib/SwerveDriveTest.java

@@ -360,7 +360,8 @@ public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog
     double velocity = module.getAbsoluteEncoder().getVelocity();
     SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Power", power);
     SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Position", angle);
-    SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity", velocity);
+    SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity",
+                             velocity);
     log.motor("angle-" + module.configuration.name)
        .voltage(m_appliedVoltage.mut_replace(power, Volts))
        .angularPosition(m_anglePosition.mut_replace(angle, Degrees))

src/main/java/swervelib/SwerveModule.java

@@ -379,8 +379,10 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
 
     if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
     {
-      SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Speed Setpoint", desiredState.speedMetersPerSecond);
-      SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Angle Setpoint", desiredState.angle.getDegrees());
+      SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Speed Setpoint",
+                               desiredState.speedMetersPerSecond);
+      SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Angle Setpoint",
+                               desiredState.angle.getDegrees());
     }
   }
 

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

@@ -410,4 +410,16 @@ public static void antiJitter(SwerveModuleState moduleState, SwerveModuleState l
       moduleState.angle = lastModuleState.angle;
     }
   }
+
+  /**
+   * Get the scaled {@link Translation2d} with the given scalar to change the magnitude of the {@link Vector2d}.
+   *
+   * @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.
+   */
+  public static Translation2d scaleTranslation2d(Translation2d cartesian, double scalar)
+  {
+    return new Vector2d(cartesian).scale(scalar).getTranslation();
+  }
 }

src/main/java/swervelib/math/Vector2d.java

@@ -0,0 +1,96 @@
+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));
+  }
+}