first few error fixes

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 26d35a7de384d3e2bbd8aa85ad34a8d73687ab71..5d219392ffa4aa2b3c1bc4e449f869704adb623f 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -149,7 +149,7 @@ public class RobotContainer {
         new AutoFactory(
             drive::getPose,
             drive::resetOdometry,
-            sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false),
+            sample -> drive.setChassisVelocities(sample.getChassisVelocities(), false),
             true,
             drive,
             (trajectory, startOrFinish) -> {
@@ -172,12 +172,12 @@ public class RobotContainer {
         (pose) -> {
           drive.resetOdometry(pose);
         },
-        () -> drive.getChassisSpeeds(),
+        () -> drive.getChassisVelocities(),
         (chassisSpeeds) -> {
           if (!Constants.DISABLE_LOGGING) {
-            Logger.recordOutput("Auto/ChassisSpeeds", chassisSpeeds);
+            Logger.recordOutput("Auto/ChassisVelocities", chassisSpeeds);
           }
-          drive.setChassisSpeeds(chassisSpeeds, false); // problem??
+          drive.setChassisVelocities(chassisSpeeds, false); // problem??
         },
         AutoConstants.AUTO_CONTROLLER,
         AutoConstants.CONFIG,

diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java
index 888e1a1a157b44c99f96eb9c68a2a0a9a65f0833..393aec62db39027c732a6e7c13c4e092fe7108de 100644 (file)
--- a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java
+++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java
@@ -1,21 +1,22 @@
-package frc.robot.commands.auto_comm;
+// TODO: 2027-ALPHA-FIX - Re-enable Choreo vendordep when JSON is updated
+// package frc.robot.commands.auto_comm;
 
-import choreo.auto.AutoFactory;
-import org.wpilib.command2.Command;
-import org.wpilib.command2.Commands;
-import org.wpilib.command2.InstantCommand;
-import frc.robot.commands.DoNothing;
+// import choreo.auto.AutoFactory;
+// import org.wpilib.command2.Command;
+// import org.wpilib.command2.Commands;
+// import org.wpilib.command2.InstantCommand;
+// import frc.robot.commands.DoNothing;
 
-public class ChoreoPathCommandBuilder {
+// public class ChoreoPathCommandBuilder {
 
-  public ChoreoPathCommandBuilder() {}
+//   public ChoreoPathCommandBuilder() {}
 
-  public static Command basicTrajectoryAuto(
-      String pathName, boolean resetOdemetry, AutoFactory factory) {
-    Command command = factory.trajectoryCmd(pathName);
+//   public static Command basicTrajectoryAuto(
+//       String pathName, boolean resetOdemetry, AutoFactory factory) {
+//     Command command = factory.trajectoryCmd(pathName);
 
-    return Commands.sequence(
-        resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(),
-        command);
-  }
-}
+//     return Commands.sequence(
+//         resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(),
+//         command);
+//   }
+// }

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 5ddaa9ad3effc945f7533a8e0231a9028b1ec69d..a25c8a72cb991c9bf70e28be9fe25856b171810c 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
@@ -1,7 +1,7 @@
 package frc.robot.commands.drive_comm;
 
 import org.wpilib.math.controller.PIDController;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.driverstation.DriverStation.Alliance;
 import org.wpilib.smartdashboard.SmartDashboard;
 import org.wpilib.command2.Command;

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 d2331dba5af26345ea812cc012454e8d6aaa9c6e..7c6d014ec5cc91b411506d2fd280a3e6d3ac5086 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java
@@ -16,7 +16,7 @@ import org.wpilib.math.filter.Debouncer;
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Translation2d;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.math.trajectory.TrapezoidProfile;
 import org.wpilib.math.util.Units;
 import org.wpilib.command2.Command;
@@ -102,8 +102,8 @@ public class DriveToPose extends Command {
 
     targetPose = target.get();
     Pose2d currentPose = robot.get();
-    ChassisSpeeds fieldVelocity =
-        ChassisSpeeds.fromRobotRelativeSpeeds(drive.getChassisSpeeds(), currentPose.getRotation());
+    ChassisVelocities fieldVelocity =
+        ChassisVelocities.fromRobotRelativeSpeeds(drive.getChassisVelocities(), currentPose.getRotation());
     Translation2d linearFieldVelocity =
         new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond);
 
@@ -141,7 +141,7 @@ public class DriveToPose extends Command {
     // Calculate drive speed
     double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation());
     double ffScaler =
-        MathUtil.clamp((currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius), 0.0, 1.0);
+        Math.max(0.0, Math.min(1.0,(currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius)));
     driveErrorAbs = currentDistance;
     driveController.reset(
         lastSetpointTranslation.getDistance(targetPose.getTranslation()),

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 87b77a40cd3d6440671bc81c6668a192fe2d7300..62abbe6f0eb2639292417524a94e5ce73224e1c5 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java
+++ b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java
@@ -1,7 +1,7 @@
 package frc.robot.commands.drive_comm;
 
 import org.wpilib.math.geometry.Pose2d;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.math.kinematics.SwerveModuleState;
 import org.wpilib.math.trajectory.Trajectory;
 import org.wpilib.math.trajectory.Trajectory.State;
@@ -34,9 +34,9 @@ public class TrajectoryPresetSteerAngles extends InstantCommand {
           double angularVelo =
               (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time;
 
-          ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelo);
+          ChassisVelocities chassisSpeeds = new ChassisVelocities(xVelocity, yVelocity, angularVelo);
           chassisSpeeds =
-              ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation());
+              ChassisVelocities.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation());
 
           SwerveModuleState[] swerveModuleStates =
               DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds);

diff --git a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java
index 7479b043d46dd4c8037008a83e613ea4407bed54..b83303af0bf0b43b8880c0f1833f06aaf3548fa0 100644 (file)
--- a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java
+++ b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java
@@ -3,7 +3,7 @@ package frc.robot.commands.vision;
 import java.util.function.Supplier;
 
 import org.wpilib.math.util.MathUtil;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 import frc.robot.commands.drive_comm.DefaultDriveCommand;
 import frc.robot.constants.VisionConstants;
 import frc.robot.controls.BaseDriverConfig;
@@ -29,7 +29,7 @@ public class AimAtGamePiece extends DefaultDriveCommand {
   }
 
   @Override
-  protected void drive(ChassisSpeeds speeds) {
+  protected void drive(ChassisVelocities speeds) {
     if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
       super.drive(speeds);
       return;

diff --git a/src/main/java/frc/robot/commands/vision/GoToPose2.java b/src/main/java/frc/robot/commands/vision/GoToPose2.java
index 72c2a351f926dae8054014197a713bb274ee8149..974ea57308975b386fab101091a0607b6881554d 100644 (file)
--- a/src/main/java/frc/robot/commands/vision/GoToPose2.java
+++ b/src/main/java/frc/robot/commands/vision/GoToPose2.java
@@ -4,7 +4,7 @@ import java.util.function.Supplier;
 
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Translation2d;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.command2.Command;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.drivetrain.Drivetrain;
@@ -27,7 +27,7 @@ public class GoToPose2 extends Command {
   @Override
   public void initialize() {
     pose = poseSupplier.get();
-    ChassisSpeeds v = drive.getChassisSpeeds();
+    ChassisVelocities v = drive.getChassisVelocities();
     vx = v.vxMetersPerSecond;
     vy = v.vyMetersPerSecond;
     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 0bc2f0b751f30408dcb1257ca8bfaaa6ea978a4f..809c5548ce573c777bb1edab2213c2a91b95271b 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -19,7 +19,7 @@ import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Translation2d;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.math.kinematics.SwerveDriveKinematics;
 import org.wpilib.math.kinematics.SwerveModulePosition;
 import org.wpilib.math.kinematics.SwerveModuleState;
@@ -62,7 +62,7 @@ public class Drivetrain extends SubsystemBase {
 
   private SwerveSetpoint currentSetpoint =
       new SwerveSetpoint(
-          new ChassisSpeeds(),
+          new ChassisVelocities(),
           new SwerveModuleState[] {
             new SwerveModuleState(),
             new SwerveModuleState(),
@@ -280,11 +280,11 @@ public class Drivetrain extends SubsystemBase {
   public void drive(
       double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) {
     // rot = headingControl(rot, xSpeed, ySpeed);
-    ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME);
+    ChassisVelocities speeds = ChassisVelocities.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME);
     if (fieldRelative) {
-      speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+      speeds = ChassisVelocities.fromFieldRelativeSpeeds(speeds, getYaw());
     }
-    setChassisSpeeds(speeds, isOpenLoop);
+    setChassisVelocities(speeds, isOpenLoop);
   }
 
   /**
@@ -297,11 +297,11 @@ public class Drivetrain extends SubsystemBase {
    */
   public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
     double rot = rotationController.calculate(getYaw().getRadians(), heading);
-    ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
+    ChassisVelocities speeds = new ChassisVelocities(xSpeed, ySpeed, rot);
     if (fieldRelative) {
-      speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+      speeds = ChassisVelocities.fromFieldRelativeSpeeds(speeds, getYaw());
     }
-    setChassisSpeeds(speeds, false);
+    setChassisVelocities(speeds, false);
   }
 
   /**
@@ -480,10 +480,10 @@ public class Drivetrain extends SubsystemBase {
    * @param chassisSpeeds the target chassis speeds
    * @param isOpenLoop if open loop control should be used for the drive velocity
    */
-  public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) {
+  public void setChassisVelocities(ChassisVelocities chassisSpeeds, boolean isOpenLoop) {
 
     if (DriveConstants.USE_ACTUAL_SPEED) {
-      SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates());
+      SwerveSetpoint currentState = new SwerveSetpoint(getChassisVelocities(), getModuleStates());
       currentSetpoint =
           setpointGenerator.generateSetpoint(
               DriveConstants.MODULE_LIMITS,
@@ -589,10 +589,10 @@ public class Drivetrain extends SubsystemBase {
   /**
    * Calculates chassis speed of drivetrain using the current SwerveModuleStates
    *
-   * @return ChassisSpeeds object This is often used as an input for other methods
+   * @return ChassisVelocities object This is often used as an input for other methods
    */
-  public ChassisSpeeds getChassisSpeeds() {
-    return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates());
+  public ChassisVelocities getChassisVelocities() {
+    return DriveConstants.KINEMATICS.toChassisVelocities(getModuleStates());
   }
 
   /**

diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java
index 4fb46d6573fe8e44b14fa4941aa3e1abce706d6d..67e67b5fb19595c0cf7a9e1098d9a96a8adbf729 100644 (file)
--- a/src/main/java/frc/robot/util/GeomUtil.java
+++ b/src/main/java/frc/robot/util/GeomUtil.java
@@ -14,7 +14,7 @@ import org.wpilib.math.geometry.Transform2d;
 import org.wpilib.math.geometry.Transform3d;
 import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.geometry.Twist2d;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 
 /** Geometry utilities for working with translations, rotations, transforms, and poses. */
 public class GeomUtil {
@@ -129,12 +129,12 @@ public class GeomUtil {
   }
 
   /**
-   * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain
+   * Converts a ChassisVelocities to a Twist2d by extracting two dimensions (Y and Z). chain
    *
    * @param speeds The original translation
    * @return The resulting translation
    */
-  public static Twist2d toTwist2d(ChassisSpeeds speeds) {
+  public static Twist2d toTwist2d(ChassisVelocities speeds) {
     return new Twist2d(
         speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
   }

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

diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
index 1d2a3db8073814387ffb17a124acb1ac34d12853..8039f56e633870363387841937324427cb03466b 100644 (file)
--- a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
+++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java
@@ -12,7 +12,7 @@ import static frc.robot.util.EqualsUtil.*;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.geometry.Twist2d;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.math.kinematics.SwerveDriveKinematics;
 import org.wpilib.math.kinematics.SwerveModuleState;
 import java.util.ArrayList;
@@ -27,7 +27,7 @@ import frc.robot.util.GeomUtil;
 /**
  * "Inspired" by FRC team 254. See the license file in the root directory of this project.
  *
- * <p>Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path
+ * <p>Takes a prior setpoint (ChassisVelocities), a desired setpoint (from a driver, or from a path
  * follower), and outputs a new setpoint that respects all of the kinematic constraints on module
  * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the
  * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is
@@ -262,7 +262,7 @@ public class SwerveSetpointGenerator {
       final ModuleLimits limits,
       double centerOfMassHeight,
       final SwerveSetpoint prevSetpoint,
-      ChassisSpeeds desiredState,
+      ChassisVelocities desiredState,
       double dt) {
     final Translation2d[] modules = moduleLocations;
 
@@ -270,7 +270,7 @@ public class SwerveSetpointGenerator {
     // Make sure desiredState respects velocity limits.
     if (limits.maxDriveVelocity() > 0.0) {
       SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity());
-      desiredState = kinematics.toChassisSpeeds(desiredModuleState);
+      desiredState = kinematics.toChassisVelocities(desiredModuleState);
     }
 
     // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so
@@ -327,7 +327,7 @@ public class SwerveSetpointGenerator {
       // It will (likely) be faster to stop the robot, rotate the modules in place to the complement
       // of the desired
       // angle, and accelerate again.
-      return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisSpeeds(), dt);
+      return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisVelocities(), dt);
     }
 
     // Compute the deltas between start and goal. We can then interpolate from the start state to
@@ -470,8 +470,8 @@ public class SwerveSetpointGenerator {
       }
     }
 
-    ChassisSpeeds retSpeeds =
-        new ChassisSpeeds(
+    ChassisVelocities retSpeeds =
+        new ChassisVelocities(
             prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx,
             prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy,
             prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta);

diff --git a/src/main/java/frc/robot/util/Vision/DetectedObject.java b/src/main/java/frc/robot/util/Vision/DetectedObject.java
index f689438b060793d86d5ea84b6d1036dd138e8391..cc0f81d983e212d0f810396ffe7e947fd8a9d509 100644 (file)
--- a/src/main/java/frc/robot/util/Vision/DetectedObject.java
+++ b/src/main/java/frc/robot/util/Vision/DetectedObject.java
@@ -6,7 +6,7 @@ import org.wpilib.math.geometry.Pose3d;
 import org.wpilib.math.geometry.Rotation3d;
 import org.wpilib.math.geometry.Transform3d;
 import org.wpilib.math.geometry.Translation3d;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.math.util.Units;
 import org.wpilib.driverstation.DriverStation.Alliance;
 import frc.robot.Robot;
@@ -370,7 +370,7 @@ public class DetectedObject {
    * @return The relative angle in radians
    */
   public double getVelocityRelativeAngle() {
-    ChassisSpeeds speeds = drive.getChassisSpeeds();
+    ChassisVelocities speeds = drive.getChassisVelocities();
     double angle =
         getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond);
     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 8ebf7feca0302716a55485403cd69b37321a0e22..c50c9ffce0ef7191e872601bfd30583cac6397e8 100644 (file)
--- a/src/main/java/frc/robot/util/Vision/DriverAssist.java
+++ b/src/main/java/frc/robot/util/Vision/DriverAssist.java
@@ -8,7 +8,7 @@ import org.wpilib.math.util.MathUtil;
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Translation2d;
-import org.wpilib.math.kinematics.ChassisSpeeds;
+import org.wpilib.math.kinematics.ChassisVelocities;
 import org.wpilib.math.trajectory.TrapezoidProfile;
 import org.wpilib.math.trajectory.TrapezoidProfile.Constraints;
 import org.wpilib.math.trajectory.TrapezoidProfile.State;
@@ -51,8 +51,8 @@ public class DriverAssist {
    * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
    * @return The new speed
    */
-  private static ChassisSpeeds calculate2(
-      Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+  private static ChassisVelocities calculate2(
+      Drivetrain drive, ChassisVelocities driverInput, Pose2d desiredPose, boolean keepAngle) {
     // Do nothing if there is no pose
     if (desiredPose == null) {
       return driverInput;
@@ -61,11 +61,11 @@ public class DriverAssist {
     // Store current states
     Pose2d currentPose = drive.getPose();
     Rotation2d yaw = drive.getYaw();
-    ChassisSpeeds driveSpeeds = drive.getChassisSpeeds();
+    ChassisVelocities driveSpeeds = drive.getChassisVelocities();
     driveSpeeds =
-        ChassisSpeeds.fromFieldRelativeSpeeds(
+        ChassisVelocities.fromFieldRelativeSpeeds(
             driveSpeeds,
-            yaw); // Changing this does not cause problems because getChassisSpeeds() creates a new
+            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);
@@ -86,14 +86,14 @@ public class DriverAssist {
     State angleGoal = new State(rotation, 0);
 
     // Calculate ideal speeds for next frame
-    ChassisSpeeds goal =
-        new ChassisSpeeds(
+    ChassisVelocities goal =
+        new ChassisVelocities(
             xProfile.calculate(Constants.LOOP_TIME, xState, xGoal).velocity,
             yProfile.calculate(Constants.LOOP_TIME, yState, yGoal).velocity,
             angleProfile.calculate(Constants.LOOP_TIME, angleState, angleGoal).velocity);
     // Robot-relataive goal
-    ChassisSpeeds goalRobot = goal.times(1);
-    goalRobot = ChassisSpeeds.fromRobotRelativeSpeeds(goalRobot, yaw);
+    ChassisVelocities goalRobot = goal.times(1);
+    goalRobot = ChassisVelocities.fromRobotRelativeSpeeds(goalRobot, yaw);
 
     // This calculates the actual acceleration we can get
     // This is the only thing that needs to be robot relative
@@ -104,12 +104,12 @@ public class DriverAssist {
             drive.getCurrSetpoint(),
             goalRobot,
             Constants.LOOP_TIME);
-    ChassisSpeeds nextChassisSpeed = nextSetpoint.chassisSpeeds();
-    nextChassisSpeed = ChassisSpeeds.fromRobotRelativeSpeeds(nextChassisSpeed, yaw);
+    ChassisVelocities nextChassisSpeed = nextSetpoint.chassisSpeeds();
+    nextChassisSpeed = ChassisVelocities.fromRobotRelativeSpeeds(nextChassisSpeed, yaw);
 
     // Robot relative driver inputs
-    ChassisSpeeds driverInputRobot = driverInput.times(1); // Copy so original doesn't change
-    driverInputRobot = ChassisSpeeds.fromFieldRelativeSpeeds(driverInputRobot, yaw);
+    ChassisVelocities driverInputRobot = driverInput.times(1); // Copy so original doesn't change
+    driverInputRobot = ChassisVelocities.fromFieldRelativeSpeeds(driverInputRobot, yaw);
     // This is the speed the driver will be able to get next frame
     // Both speeds need to be obtainable in 1 frame or the driver speed will always be farther away
     SwerveSetpoint driverSetpoint =
@@ -119,11 +119,11 @@ public class DriverAssist {
             drive.getCurrSetpoint(),
             driverInputRobot,
             Constants.LOOP_TIME);
-    ChassisSpeeds driverSpeeds = driverSetpoint.chassisSpeeds();
-    driverSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(driverSpeeds, yaw);
+    ChassisVelocities driverSpeeds = driverSetpoint.chassisSpeeds();
+    driverSpeeds = ChassisVelocities.fromRobotRelativeSpeeds(driverSpeeds, yaw);
 
     // The difference between the 2 speeds
-    ChassisSpeeds error = nextChassisSpeed.minus(driverSpeeds);
+    ChassisVelocities error = nextChassisSpeed.minus(driverSpeeds);
 
     // 1.2*1.2^-distance decreases the amount it correct by as distance increases
     double distanceFactor =
@@ -136,7 +136,7 @@ public class DriverAssist {
         Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
 
     // The amount to correct by
-    ChassisSpeeds correction =
+    ChassisVelocities correction =
         error.times(
             Math.min(
                 CORRECTION_FACTOR * distanceFactor * driverInputSpeed / DriveConstants.MAX_SPEED,
@@ -164,8 +164,8 @@ public class DriverAssist {
    */
   @SuppressWarnings(
       "unused") // Needed because some code might not run for some values of DRIVER_ASSIST_MODE
-  public static ChassisSpeeds calculate(
-      Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+  public static ChassisVelocities calculate(
+      Drivetrain drive, ChassisVelocities driverInput, Pose2d desiredPose, boolean keepAngle) {
     if (VisionConstants.DRIVER_ASSIST_MODE < 2 || desiredPose == null) {
       return driverInput;
     } else if (VisionConstants.DRIVER_ASSIST_MODE == 2) {
@@ -231,7 +231,7 @@ public class DriverAssist {
                 * Math.sqrt(2 * DriveConstants.MAX_ANGULAR_ACCEL * Math.abs(rotationError))
             - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond;
     return driverInput.plus(
-        new ChassisSpeeds(
+        new ChassisVelocities(
             correctionSpeed * Math.cos(perpendicularAngle),
             correctionSpeed * Math.sin(perpendicularAngle),
             rotationalSpeed));