I love api changes

diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
index a25c8a72cb991c9bf70e28be9fe25856b171810c..f955075fa4f668097c47b53da1cef39895918629 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
@@ -53,27 +53,27 @@ public class DefaultDriveCommand extends Command {
     forwardTranslation *= allianceReversal;
     sideTranslation *= allianceReversal;
 
-    ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation);
-    ChassisSpeeds corrected =
+    ChassisVelocities driverInput = new ChassisVelocities(forwardTranslation, sideTranslation, rotation);
+    ChassisVelocities corrected =
         DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true);
   }
 
   /**
    * Drives the robot
    *
-   * @param speeds The ChassisSpeeds to drive at
+   * @param speeds The ChassisVelocities to drive at
    */
-  protected void drive(ChassisSpeeds speeds) {
+  protected void drive(ChassisVelocities speeds) {
     // If the driver is pressing the align button or a command set the drivetrain to
     // align, then align to speaker
     if (driver.getIsAlign() || swerve.getIsAlign()) {
       swerve.driveHeading(
-          speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, swerve.getAlignAngle(), true);
+          speeds.vx, speeds.vy, swerve.getAlignAngle(), true);
     } else {
       swerve.drive(
-          speeds.vxMetersPerSecond,
-          speeds.vyMetersPerSecond,
-          speeds.omegaRadiansPerSecond,
+          speeds.vx,
+          speeds.vy,
+          speeds.omega,
           true,
           false);
     }

diff --git a/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java
index 7c6d014ec5cc91b411506d2fd280a3e6d3ac5086..f5c2a0d49f8894a523e7ce266c4c6d6761565654 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java
@@ -105,10 +105,10 @@ public class DriveToPose extends Command {
     ChassisVelocities fieldVelocity =
         ChassisVelocities.fromRobotRelativeSpeeds(drive.getChassisVelocities(), currentPose.getRotation());
     Translation2d linearFieldVelocity =
-        new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond);
+        new Translation2d(fieldVelocity.vx, fieldVelocity.vy);
 
     thetaController.reset(
-        currentPose.getRotation().getRadians(), fieldVelocity.omegaRadiansPerSecond);
+        currentPose.getRotation().getRadians(), fieldVelocity.omega);
     lastSetpointTranslation = currentPose.getTranslation();
 
     if (targetPose != null) {

diff --git a/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java
index 37bd539731222f8cc58b8688632da34646cf26ed..7d6ba318ff7866b79b8dda67f3f53c0976eeff38 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java
+++ b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java
@@ -1,7 +1,8 @@
 package frc.robot.commands.drive_comm;
 
 import org.wpilib.math.geometry.Rotation2d;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 import org.wpilib.math.util.Units;
 import org.wpilib.command2.InstantCommand;
 import org.wpilib.command2.RunCommand;
@@ -17,11 +18,11 @@ public class SetFormationX extends SequentialCommandGroup {
         new RunCommand(
             () ->
                 drive.setModuleStates(
-                    new SwerveModuleState[] {
-                      new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))),
-                      new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
-                      new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
-                      new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45)))
+                    new SwerveModuleVelocity[] {
+                      new SwerveModuleVelocity(0, new Rotation2d(Units.degreesToRadians(45))),
+                      new SwerveModuleVelocity(0, new Rotation2d(Units.degreesToRadians(-45))),
+                      new SwerveModuleVelocity(0, new Rotation2d(Units.degreesToRadians(-45))),
+                      new SwerveModuleVelocity(0, new Rotation2d(Units.degreesToRadians(45)))
                     },
                     false),
             drive));

diff --git a/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java
index 4fba8b1dacd877dbb5d7e278fee6e27b7eb05562..a9602e5045bbb4546f981278daf299df3a8ac489 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java
+++ b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java
@@ -1,7 +1,7 @@
 package frc.robot.commands.drive_comm;
 
 import org.wpilib.math.geometry.Rotation2d;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 import org.wpilib.command2.InstantCommand;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 
@@ -41,11 +41,11 @@ public class SimplePresetSteerAngles extends InstantCommand {
         () -> {
           drive.setStateDeadband(false);
           drive.setModuleStates(
-              new SwerveModuleState[] {
-                new SwerveModuleState(0, rotation),
-                new SwerveModuleState(0, rotation),
-                new SwerveModuleState(0, rotation),
-                new SwerveModuleState(0, rotation)
+              new SwerveModuleVelocity[] {
+                new SwerveModuleVelocity(0, rotation),
+                new SwerveModuleVelocity(0, rotation),
+                new SwerveModuleVelocity(0, rotation),
+                new SwerveModuleVelocity(0, rotation)
               },
               true);
           drive.setStateDeadband(true);

diff --git a/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java
index 62abbe6f0eb2639292417524a94e5ce73224e1c5..3ba8916e2b312f2f28e704764ca54a9e962fa3e5 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java
+++ b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java
@@ -2,7 +2,7 @@ package frc.robot.commands.drive_comm;
 
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.kinematics.ChassisVelocities;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 import org.wpilib.math.trajectory.Trajectory;
 import org.wpilib.math.trajectory.Trajectory.State;
 import org.wpilib.command2.InstantCommand;
@@ -29,8 +29,8 @@ public class TrajectoryPresetSteerAngles extends InstantCommand {
           State sample = trajectory.sample(time);
           Pose2d nextPose = sample.poseMeters;
 
-          double xVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getCos();
-          double yVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getSin();
+          double xVelocity = sample.velocity * nextPose.getRotation().getCos();
+          double yVelocity = sample.velocity * nextPose.getRotation().getSin();
           double angularVelo =
               (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time;
 
@@ -38,12 +38,12 @@ public class TrajectoryPresetSteerAngles extends InstantCommand {
           chassisSpeeds =
               ChassisVelocities.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation());
 
-          SwerveModuleState[] swerveModuleStates =
-              DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds);
-          for (SwerveModuleState swerveModuleState : swerveModuleStates) {
-            swerveModuleState.speedMetersPerSecond = 0;
+          SwerveModuleVelocity[] SwerveModuleVelocitys =
+              DriveConstants.KINEMATICS.toSwerveModuleVelocitys(chassisSpeeds);
+          for (SwerveModuleVelocity SwerveModuleVelocity : SwerveModuleVelocitys) {
+            SwerveModuleVelocity.speedMetersPerSecond = 0;
           }
-          drive.setModuleStates(swerveModuleStates, true);
+          drive.setModuleStates(SwerveModuleVelocitys, true);
           drive.setStateDeadband(true);
         },
         drive);

diff --git a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java
index b83303af0bf0b43b8880c0f1833f06aaf3548fa0..506c6199033487d854f9ec68240a0c14f8160d60 100644 (file)
--- a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java
+++ b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java
@@ -51,8 +51,8 @@ public class AimAtGamePiece extends DefaultDriveCommand {
 
     // System.out.println("objangle " + object.getAngle());
     swerve.driveHeading(
-        speeds.vxMetersPerSecond,
-        speeds.vyMetersPerSecond,
+        speeds.vx,
+        speeds.vy,
         MathUtil.angleModulus(object.getAngle()),
         true);
   }

diff --git a/src/main/java/frc/robot/commands/vision/GoToPose2.java b/src/main/java/frc/robot/commands/vision/GoToPose2.java
index 974ea57308975b386fab101091a0607b6881554d..c42be32bcb3c5852d566640cf49134b641b1b6c5 100644 (file)
--- a/src/main/java/frc/robot/commands/vision/GoToPose2.java
+++ b/src/main/java/frc/robot/commands/vision/GoToPose2.java
@@ -28,8 +28,8 @@ public class GoToPose2 extends Command {
   public void initialize() {
     pose = poseSupplier.get();
     ChassisVelocities v = drive.getChassisVelocities();
-    vx = v.vxMetersPerSecond;
-    vy = v.vyMetersPerSecond;
+    vx = v.vx;
+    vy = v.vy;
     error = null;
   }
 

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 809c5548ce573c777bb1edab2213c2a91b95271b..1e2d12c08f069da553e89722c30b9a9ae77bb403 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -22,7 +22,7 @@ import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.math.kinematics.SwerveDriveKinematics;
 import org.wpilib.math.kinematics.SwerveModulePosition;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 import org.wpilib.math.util.Units;
 import org.wpilib.units.measure.Voltage;
 import org.wpilib.framework.RobotBase;
@@ -63,11 +63,11 @@ public class Drivetrain extends SubsystemBase {
   private SwerveSetpoint currentSetpoint =
       new SwerveSetpoint(
           new ChassisVelocities(),
-          new SwerveModuleState[] {
-            new SwerveModuleState(),
-            new SwerveModuleState(),
-            new SwerveModuleState(),
-            new SwerveModuleState()
+          new SwerveModuleVelocity[] {
+            new SwerveModuleVelocity(),
+            new SwerveModuleVelocity(),
+            new SwerveModuleVelocity(),
+            new SwerveModuleVelocity()
           });
   // Odometry
   private final SwerveDrivePoseEstimator poseEstimator;
@@ -421,7 +421,7 @@ public class Drivetrain extends SubsystemBase {
 
     // if (Robot.isSimulation()) {
     // pigeon.getSimState().addYaw(
-    // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond
+    // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omega
     // * Constants.LOOP_TIME));
     // }
   }
@@ -462,15 +462,15 @@ public class Drivetrain extends SubsystemBase {
   /**
    * Sets the desired states for all swerve modules.
    *
-   * @param swerveModuleStates an array of module states to set swerve modules to. Order of the
+   * @param SwerveModuleVelocitys an array of module states to set swerve modules to. Order of the
    *     array matters here!
    */
-  public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) {
+  public void setModuleStates(SwerveModuleVelocity[] SwerveModuleVelocitys, boolean isOpenLoop) {
     // makes sure speeds of modules don't exceed maximum allowed
-    SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED);
+    SwerveDriveKinematics.desaturateWheelSpeeds(SwerveModuleVelocitys, DriveConstants.MAX_SPEED);
 
     for (int i = 0; i < 4; i++) {
-      modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop);
+      modules[i].setDesiredState(SwerveModuleVelocitys[i], isOpenLoop);
     }
   }
 
@@ -501,8 +501,8 @@ public class Drivetrain extends SubsystemBase {
               Constants.LOOP_TIME);
     }
 
-    SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates();
-    setModuleStates(swerveModuleStates, isOpenLoop);
+    SwerveModuleVelocity[] SwerveModuleVelocitys = currentSetpoint.moduleStates();
+    setModuleStates(SwerveModuleVelocitys, isOpenLoop);
   }
 
   public void setDriveVoltages(Voltage voltage) {
@@ -587,7 +587,7 @@ public class Drivetrain extends SubsystemBase {
   }
 
   /**
-   * Calculates chassis speed of drivetrain using the current SwerveModuleStates
+   * Calculates chassis speed of drivetrain using the current SwerveModuleVelocitys
    *
    * @return ChassisVelocities object This is often used as an input for other methods
    */
@@ -598,10 +598,10 @@ public class Drivetrain extends SubsystemBase {
   /**
    * Gets the state of each module
    *
-   * @return An array of 4 SwerveModuleStates
+   * @return An array of 4 SwerveModuleVelocitys
    */
-  public SwerveModuleState[] getModuleStates() {
-    return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new);
+  public SwerveModuleVelocity[] getModuleStates() {
+    return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleVelocity[]::new);
   }
 
   public SwerveSetpoint getCurrSetpoint() {
@@ -843,7 +843,7 @@ public class Drivetrain extends SubsystemBase {
   }
 
   public void alignWheels() {
-    SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0));
-    setModuleStates(new SwerveModuleState[] {state, state, state, state}, false);
+    SwerveModuleVelocity state = new SwerveModuleVelocity(0, new Rotation2d(0));
+    setModuleStates(new SwerveModuleVelocity[] {state, state, state, state}, false);
   }
 }

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java
index 9caf1d64aa23ab59b0a729522a9002579daf2e6d..1affc0d1ac70f16214a58682d13d2fccaeed9f88 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Module.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java
@@ -26,7 +26,7 @@ import org.wpilib.math.util.MathUtil;
 import org.wpilib.math.filter.Debouncer;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.kinematics.SwerveModulePosition;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 import org.wpilib.math.util.Units;
 import org.wpilib.units.measure.Angle;
 import frc.robot.constants.Constants;
@@ -50,7 +50,7 @@ public class Module implements ModuleIO {
   private final TalonFX angleMotor;
   private final TalonFX driveMotor;
   private final CANcoder CANcoder;
-  private SwerveModuleState desiredState;
+  private SwerveModuleVelocity desiredState;
 
   protected boolean stateDeadband = true;
 
@@ -158,7 +158,7 @@ public class Module implements ModuleIO {
         turnAppliedVolts,
         turnCurrent);
 
-    setDesiredState(new SwerveModuleState(0, getAngle()), false);
+    setDesiredState(new SwerveModuleVelocity(0, getAngle()), false);
   }
 
   public void close() {
@@ -252,7 +252,7 @@ public class Module implements ModuleIO {
     }
   }
 
-  public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
+  public void setDesiredState(SwerveModuleVelocity wantedState, boolean isOpenLoop) {
     // Separate if here and in setAngle() to avoid warning
     if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
       /*
@@ -481,8 +481,8 @@ public class Module implements ModuleIO {
     driveMotor.optimizeBusUtilization();
   }
 
-  public SwerveModuleState getState() {
-    return new SwerveModuleState(
+  public SwerveModuleVelocity getState() {
+    return new SwerveModuleVelocity(
         inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS, getAngle());
   }
 
@@ -491,7 +491,7 @@ public class Module implements ModuleIO {
         inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS, getAngle());
   }
 
-  public SwerveModuleState getDesiredState() {
+  public SwerveModuleVelocity getDesiredState() {
     return desiredState;
   }
 

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java
index f6a3d6be3c3af6714bb7a9ddd2ea069eb10a9ffb..c9f395c9d0a49de9453783d0d4eaa755966c4dbc 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java
@@ -4,7 +4,7 @@ import com.ctre.phoenix6.hardware.TalonFX;
 
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.kinematics.SwerveModulePosition;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 import org.wpilib.system.Timer;
 import frc.robot.constants.Constants;
 import frc.robot.constants.swerve.DriveConstants;
@@ -21,7 +21,7 @@ public class ModuleSim extends Module {
   private double currentDrivePositionMeters = 0;
   private double currentSpeed = 0;
 
-  private SwerveModuleState desiredState;
+  private SwerveModuleVelocity desiredState;
 
   protected boolean stateDeadband = true;
 
@@ -71,7 +71,7 @@ public class ModuleSim extends Module {
    * @param desiredState Desired state with speed and angle.
    * @param isOpenLoop whether to use closed/open loop control for drive velocity
    */
-  public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
+  public void setDesiredState(SwerveModuleVelocity desiredState, boolean isOpenLoop) {
     if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
       // If the module isn't moving, don't rotate it
       if (Math.abs(desiredState.speedMetersPerSecond) < 0.001) {
@@ -91,7 +91,7 @@ public class ModuleSim extends Module {
     // does nothing when robot does not have a swerve drivetrain
   }
 
-  public SwerveModuleState getDesiredState() {
+  public SwerveModuleVelocity getDesiredState() {
     return desiredState;
   }
 
@@ -108,8 +108,8 @@ public class ModuleSim extends Module {
     currentSpeed = 0;
   }
 
-  public SwerveModuleState getState() {
-    return new SwerveModuleState(currentSpeed, getAngle());
+  public SwerveModuleVelocity getState() {
+    return new SwerveModuleVelocity(currentSpeed, getAngle());
   }
 
   public SwerveModulePosition getPosition() {

diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java
index 67e67b5fb19595c0cf7a9e1098d9a96a8adbf729..c44a40b32696c09dadeae58fa0a3047094ccbf10 100644 (file)
--- a/src/main/java/frc/robot/util/GeomUtil.java
+++ b/src/main/java/frc/robot/util/GeomUtil.java
@@ -136,7 +136,7 @@ public class GeomUtil {
    */
   public static Twist2d toTwist2d(ChassisVelocities speeds) {
     return new Twist2d(
-        speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
+        speeds.vx, speeds.vy, speeds.omega);
   }
 
   /**

diff --git a/src/main/java/frc/robot/util/SwerveModulePose.java b/src/main/java/frc/robot/util/SwerveModulePose.java
index 4b90f0bbd1b408a6956509043b45d8fe7793ecea..ef4fc43ba08214ad1fdf8f81a031a98ff796f367 100644 (file)
--- a/src/main/java/frc/robot/util/SwerveModulePose.java
+++ b/src/main/java/frc/robot/util/SwerveModulePose.java
@@ -11,7 +11,7 @@ import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.geometry.Twist2d;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 
 /** Stores and updates the position of each module */
@@ -43,7 +43,7 @@ public class SwerveModulePose {
 
   /** Updates the module positions */
   public void update() {
-    SwerveModuleState[] states = drive.getModuleStates();
+    SwerveModuleVelocity[] states = drive.getModuleStates();
     double currentRotation = drive.getYaw().getRadians();
     double chassisRotation = currentRotation - prevRotation;
 
@@ -84,7 +84,7 @@ public class SwerveModulePose {
   /** Resets the modules to the correct positions relative to the robot */
   public void reset() {
     Pose2d chassisPose2d = drive.getPose();
-    SwerveModuleState[] states = drive.getModuleStates();
+    SwerveModuleVelocity[] states = drive.getModuleStates();
     for (int i = 0; i < 4; i++) {
       angles[i] = states[i].angle.getRadians();
       this.modulePositions[i] =

diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java
index 3dda10864f424a712d078050eed16b7f363633f8..609ff6e5859160c3fd76b62dc854125c4b8ed380 100644 (file)
--- a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java
+++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java
@@ -8,6 +8,6 @@
 package frc.robot.util.SwerveStuff;
 
 import org.wpilib.math.kinematics.ChassisVelocities;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 
-public record SwerveSetpoint(ChassisVelocities chassisSpeeds, SwerveModuleState[] moduleStates) {}
+public record SwerveSetpoint(ChassisVelocities chassisSpeeds, SwerveModuleVelocity[] moduleStates) {}

diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
index 8039f56e633870363387841937324427cb03466b..0c8f7bcfba29740f0c502d23aff57f200c8c6e67 100644 (file)
--- a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
+++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
@@ -14,7 +14,7 @@ import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.geometry.Twist2d;
 import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.math.kinematics.SwerveDriveKinematics;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
@@ -266,7 +266,7 @@ public class SwerveSetpointGenerator {
       double dt) {
     final Translation2d[] modules = moduleLocations;
 
-    SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState);
+    SwerveModuleVelocity[] desiredModuleState = kinematics.toSwerveModuleVelocitys(desiredState);
     // Make sure desiredState respects velocity limits.
     if (limits.maxDriveVelocity() > 0.0) {
       SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity());
@@ -334,10 +334,10 @@ public class SwerveSetpointGenerator {
     // the goal state; then
     // find the amount we can move from start towards goal in this cycle such that no kinematic
     // limit is exceeded.
-    double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond;
-    double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond;
+    double dx = desiredState.vx - prevSetpoint.chassisSpeeds().vx;
+    double dy = desiredState.vy - prevSetpoint.chassisSpeeds().vy;
     double dtheta =
-        desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond;
+        desiredState.omega - prevSetpoint.chassisSpeeds().omega;
 
     // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at
     // desiredState.
@@ -455,10 +455,10 @@ public class SwerveSetpointGenerator {
       // x and y are limited separately because, when tipping in a diagonal direction, the distance
       // is longer
       double xAccel =
-          Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond)
+          Math.abs(desiredState.vx - prevSetpoint.chassisSpeeds().vx)
               / dt;
       double yAccel =
-          Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond)
+          Math.abs(desiredState.vy - prevSetpoint.chassisSpeeds().vy)
               / dt;
       if (!epsilonEquals(xAccel, 0)) {
         double s = maxAccel / xAccel;
@@ -472,10 +472,10 @@ public class SwerveSetpointGenerator {
 
     ChassisVelocities retSpeeds =
         new ChassisVelocities(
-            prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx,
-            prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy,
-            prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta);
-    var retStates = kinematics.toSwerveModuleStates(retSpeeds);
+            prevSetpoint.chassisSpeeds().vx + min_s * dx,
+            prevSetpoint.chassisSpeeds().vy + min_s * dy,
+            prevSetpoint.chassisSpeeds().omega + min_s * dtheta);
+    var retStates = kinematics.toSwerveModuleVelocitys(retSpeeds);
     for (int i = 0; i < modules.length; ++i) {
       final var maybeOverride = overrideSteering.get(i);
       if (maybeOverride.isPresent()) {

diff --git a/src/main/java/frc/robot/util/Vision/DetectedObject.java b/src/main/java/frc/robot/util/Vision/DetectedObject.java
index cc0f81d983e212d0f810396ffe7e947fd8a9d509..b9f81d45f76306d2dadf2a583440c66efd76014c 100644 (file)
--- a/src/main/java/frc/robot/util/Vision/DetectedObject.java
+++ b/src/main/java/frc/robot/util/Vision/DetectedObject.java
@@ -372,7 +372,7 @@ public class DetectedObject {
   public double getVelocityRelativeAngle() {
     ChassisVelocities speeds = drive.getChassisVelocities();
     double angle =
-        getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond);
+        getRelativeAngle() - Math.atan2(speeds.vy, speeds.vx);
     return MathUtil.angleModulus(angle);
   }
 

diff --git a/src/main/java/frc/robot/util/Vision/DriverAssist.java b/src/main/java/frc/robot/util/Vision/DriverAssist.java
index c50c9ffce0ef7191e872601bfd30583cac6397e8..f3a1f0c89488353cfbdebbd71f992a23b6fc3392 100644 (file)
--- a/src/main/java/frc/robot/util/Vision/DriverAssist.java
+++ b/src/main/java/frc/robot/util/Vision/DriverAssist.java
@@ -67,10 +67,10 @@ public class DriverAssist {
             driveSpeeds,
             yaw); // Changing this does not cause problems because getChassisVelocities() creates a new
     // object
-    State xState = new State(currentPose.getX(), driveSpeeds.vxMetersPerSecond);
-    State yState = new State(currentPose.getY(), driveSpeeds.vyMetersPerSecond);
+    State xState = new State(currentPose.getX(), driveSpeeds.vx);
+    State yState = new State(currentPose.getY(), driveSpeeds.vy);
     State angleState =
-        new State(currentPose.getRotation().getRadians(), driveSpeeds.omegaRadiansPerSecond);
+        new State(currentPose.getRotation().getRadians(), driveSpeeds.omega);
 
     // Store goal states
     State xGoal = new State(desiredPose.getX(), 0);
@@ -133,7 +133,7 @@ public class DriverAssist {
 
     // Driver input speed
     double driverInputSpeed =
-        Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
+        Math.hypot(driverInput.vx, driverInput.vy);
 
     // The amount to correct by
     ChassisVelocities correction =
@@ -144,7 +144,7 @@ public class DriverAssist {
 
     return driverSpeeds.plus(correction);
     // return
-    // nextChassisSpeed.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR));
+    // nextChassisVelocities.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR));
   }
 
   // Constants used for second method
@@ -180,8 +180,8 @@ public class DriverAssist {
         keepAngle
             ? desiredPose.getRotation().getRadians()
             : MathUtil.angleModulus(velocityAngle + Math.PI / 2);
-    double inputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
-    double driverAngle = Math.atan2(driverInput.vyMetersPerSecond, driverInput.vxMetersPerSecond);
+    double inputSpeed = Math.hypot(driverInput.vx, driverInput.vy);
+    double driverAngle = Math.atan2(driverInput.vy, driverInput.vx);
     double velocityAngleError = MathUtil.angleModulus(velocityAngle - driverAngle);
     if (Math.abs(velocityAngleError) > MAX_VELOCITY_ANGLE_ERROR) {
       return driverInput;
@@ -229,7 +229,7 @@ public class DriverAssist {
         ROTATION_CORRECTION_FACTOR
                 * Math.signum(rotationError)
                 * Math.sqrt(2 * DriveConstants.MAX_ANGULAR_ACCEL * Math.abs(rotationError))
-            - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond;
+            - ROTATION_CORRECTION_FACTOR * driverInput.omega;
     return driverInput.plus(
         new ChassisVelocities(
             correctionSpeed * Math.cos(perpendicularAngle),

diff --git a/src/main/java/lib/CTREModuleState.java b/src/main/java/lib/CTREModuleState.java
index 8a90f5b095f18d54b433282e77dc8d3394dc4142..fb8742b963492f0201b505137ab09a3b9e5ee518 100644 (file)
--- a/src/main/java/lib/CTREModuleState.java
+++ b/src/main/java/lib/CTREModuleState.java
@@ -1,7 +1,7 @@
 package lib;
 
 import org.wpilib.math.geometry.Rotation2d;
-import org.wpilib.math.kinematics.SwerveModuleState;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
 
 public class CTREModuleState {
 
@@ -13,8 +13,8 @@ public class CTREModuleState {
    * @param desiredState The desired state.
    * @param currentAngle The current module angle.
    */
-  public static SwerveModuleState optimize(
-      SwerveModuleState desiredState, Rotation2d currentAngle) {
+  public static SwerveModuleVelocity optimize(
+      SwerveModuleVelocity desiredState, Rotation2d currentAngle) {
     double targetAngle =
         placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
     double targetSpeed = desiredState.speedMetersPerSecond;
@@ -27,7 +27,7 @@ public class CTREModuleState {
         targetAngle += 180;
       }
     }
-    return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
+    return new SwerveModuleVelocity(targetSpeed, Rotation2d.fromDegrees(targetAngle));
   }
 
   /**