From eb8261c0818ac3f0170580a46b32422843ef2973 Mon Sep 17 00:00:00 2001 From: Otto Date: Sun, 6 Oct 2024 22:00:01 -0400 Subject: [PATCH 01/12] Fix field rel in sim, add robot rel drive --- .vscode/settings.json | 3 +- .../frc/robot/subsystems/drive/Drive.java | 31 +++++++++++++++---- 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index b0a1681..f6985a2 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -41,5 +41,6 @@ "java.debug.settings.onBuildFailureProceed": true, "[yaml]": { "editor.defaultFormatter": "esbenp.prettier-vscode" - } + }, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9a74e36..6d0cf09 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -164,7 +164,8 @@ public void drive( DoubleSupplier omegaSupplier, double rotationMultiplier, double translationMultiplier, - boolean headingCorrection + boolean headingCorrection, + boolean fieldRelative ) { // Apply deadband double linearMagnitude = MathUtil.applyDeadband( @@ -221,8 +222,25 @@ public void drive( boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( + + if (fieldRelative) { + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + linearVelocity.getY() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, + (isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation()).times(Constants.currentMode == Constants.Mode.REAL ? -1 : 1) + ) + ); + } else { + drive.runVelocity( + ChassisSpeeds.fromRobotRelativeSpeeds( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec() * translationMultiplier, @@ -230,11 +248,12 @@ public void drive( drive.getMaxLinearSpeedMetersPerSec() * translationMultiplier, omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, - (isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation()).times(-1) + drive.getRotation() ) ); + } + + } @Override From 191f1725de32d9f41696f5c91616ef656e9490fd Mon Sep 17 00:00:00 2001 From: Otto Date: Sun, 6 Oct 2024 22:01:26 -0400 Subject: [PATCH 02/12] It builds --- src/main/java/frc/robot/subsystems/drive/Drive.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 6d0cf09..b29d113 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -128,7 +128,8 @@ public void runState() { () -> Constants.controller.getRightX(), getState().getRotationModifier(), getState().getTranslationModifier(), - headingCorrectionEnabled + headingCorrectionEnabled, + false ); } From 70e118484253d7d9ed57ee5486534ee8893fd80a Mon Sep 17 00:00:00 2001 From: github-actions Date: Mon, 7 Oct 2024 02:02:21 +0000 Subject: [PATCH 03/12] Apply Prettier format --- .../frc/robot/subsystems/drive/Drive.java | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b29d113..497cd64 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -236,25 +236,25 @@ public void drive( omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, (isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation()).times(Constants.currentMode == Constants.Mode.REAL ? -1 : 1) + : drive.getRotation()).times( + Constants.currentMode == Constants.Mode.REAL ? -1 : 1 + ) ) ); } else { drive.runVelocity( - ChassisSpeeds.fromRobotRelativeSpeeds( - linearVelocity.getX() * - drive.getMaxLinearSpeedMetersPerSec() * - translationMultiplier, - linearVelocity.getY() * - drive.getMaxLinearSpeedMetersPerSec() * - translationMultiplier, - omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, - drive.getRotation() - ) - ); + ChassisSpeeds.fromRobotRelativeSpeeds( + linearVelocity.getX() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + linearVelocity.getY() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, + drive.getRotation() + ) + ); } - - } @Override From f74061bab7b762aac5a7de012c296c32b1619770 Mon Sep 17 00:00:00 2001 From: Otto Date: Sun, 6 Oct 2024 22:51:58 -0400 Subject: [PATCH 04/12] Fix zero gyro --- .../frc/robot/subsystems/drive/GyroIONavx2.java | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavx2.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavx2.java index 4e2f20b..dbce09c 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavx2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavx2.java @@ -2,6 +2,7 @@ import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.SerialPort; import java.util.OptionalDouble; @@ -14,15 +15,18 @@ public class GyroIONavx2 implements GyroIO { private final Queue yawPositionQueue; private final Queue yawTimestampQueue; + private Rotation3d offset; + public GyroIONavx2() { navx = new AHRS(SerialPort.Port.kUSB1); - navx.enableBoardlevelYawReset(true); navx.reset(); + offset = new Rotation3d(); + yawTimestampQueue = HybridOdometryThread.getInstance().makeTimestampQueue(); yawPositionQueue = HybridOdometryThread.getInstance() .registerSignal(() -> { if (navx.isConnected()) { - return OptionalDouble.of(navx.getYaw()); + return OptionalDouble.of(navx.getRotation3d().minus(offset).getAngle()); } else { return OptionalDouble.empty(); } @@ -31,18 +35,15 @@ public GyroIONavx2() { @Override public void zero() { - System.out.println("ahhaha"); - navx.reset(); - navx.zeroYaw(); - System.out.println(navx.getYaw()); + this.offset = navx.getRotation3d(); } @Override public void updateInputs(GyroIOInputs inputs) { inputs.connected = navx.isConnected(); - inputs.yawPosition = Rotation2d.fromDegrees(navx.getYaw()); + inputs.yawPosition = Rotation2d.fromRadians(navx.getRotation3d().minus(offset).getAngle()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(navx.getRate()); - inputs.yawPosDeg = navx.getYaw(); + inputs.yawPosDeg = Units.radiansToDegrees(navx.getRotation3d().minus(offset).getAngle()); if (yawTimestampQueue != null && yawPositionQueue != null) { inputs.odometryYawTimestamps = yawTimestampQueue From 7a3db9dd7826a8d93c6835d452de79f08b8a57b0 Mon Sep 17 00:00:00 2001 From: Otto Date: Sun, 6 Oct 2024 23:01:18 -0400 Subject: [PATCH 05/12] Add field rel trigger --- src/main/java/frc/robot/subsystems/drive/Drive.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 497cd64..04cfa8e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -53,6 +53,7 @@ public class Drive extends Subsystem { private double lastHeadingRadians; private PIDController headingCorrectionController; private boolean headingCorrectionEnabled; + private boolean fieldRelative = true; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); @@ -129,13 +130,17 @@ public void runState() { getState().getRotationModifier(), getState().getTranslationModifier(), headingCorrectionEnabled, - false + fieldRelative ); } if (Constants.controller.getStartButtonPressed()) { zeroGryo(); } + + if (Constants.controller.getBackButtonPressed()) { + fieldRelative = !fieldRelative; + } } // L code??? (taken from AA) good enough From 9c13af13ad0ef634dcd7958fbba16cae14fc053a Mon Sep 17 00:00:00 2001 From: Otto Date: Sun, 6 Oct 2024 23:24:56 -0400 Subject: [PATCH 06/12] Add todos --- src/main/java/frc/robot/subsystems/drive/Drive.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 04cfa8e..e1800d7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -230,6 +230,7 @@ public void drive( DriverStation.getAlliance().get() == Alliance.Red; if (fieldRelative) { + // TODO: INVERT THE GYRO, MULTIPLYING IT BY 1 IS SO STUPID!!!!!!!!!!!!!!!!!!!!!!!!! drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( linearVelocity.getX() * From ab9a3b6a771bdd5cf52eb54301cb99aebbc3780e Mon Sep 17 00:00:00 2001 From: Otto Date: Mon, 7 Oct 2024 00:16:01 -0400 Subject: [PATCH 07/12] Remove useless SYSID stuff --- .github/scripts/constants.py | 2 +- .../java/frc/robot/subsystems/drive/Module.java | 17 ----------------- 2 files changed, 1 insertion(+), 18 deletions(-) diff --git a/.github/scripts/constants.py b/.github/scripts/constants.py index b40711e..0dcd36b 100644 --- a/.github/scripts/constants.py +++ b/.github/scripts/constants.py @@ -13,7 +13,7 @@ ] # Weird stuff that shouldn't go in constants, dont put function/var names in here theyre already checked -excused_cases = ["ModuleIOSparkMax", "case", "new Module(", "new BaseStatusSignal[", "BaseStatusSignal.waitForAll(", "new ModuleIOHybrid(", "Math.pow(", "+=", "drive.getRotation()", "autoChooser.addOption("] +excused_cases = ["ModuleIOSparkMax", "case", "new Module(", "new BaseStatusSignal[", "BaseStatusSignal.waitForAll(", "new ModuleIOHybrid(", "Math.pow(", "+=", "drive.getRotation()", "autoChooser.addOption(", "? -1 : 1"] def check_for_magic_numbers(file_path): magic_numbers = [] diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index bbacd31..96b5017 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -247,18 +247,6 @@ public SwerveModuleState runSetpoint(SwerveModuleState state) { return optimizedState; } - /** - * Runs the module with the specified voltage while controlling to zero degrees. - */ - public void runCharacterization(double volts) { - // Closed loop turn control - angleSetpoint = new Rotation2d(); - - // Open loop drive control - io.setDriveVoltage(volts); - speedSetpoint = null; - } - /** Disables all outputs to motors. */ public void stop() { io.setTurnVoltage(0.0); @@ -313,9 +301,4 @@ public SwerveModulePosition[] getOdometryPositions() { public double[] getOdometryTimestamps() { return inputs.odometryTimestamps; } - - /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; - } } From c42d0e24fa2d2e757c6942313aff07464169810a Mon Sep 17 00:00:00 2001 From: Otto Date: Mon, 7 Oct 2024 00:19:18 -0400 Subject: [PATCH 08/12] Win or lose idk --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/commands/AutoCommands.java | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7fe5e15..8c8a2e3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -223,7 +223,7 @@ public static final class Drive { public static final double FAST_TM = 2.0; // Auto Config - public static final PIDConstants TRANSLATION_PID = new PIDConstants(7.0, 0.0, 0.25); + public static final PIDConstants TRANSLATION_PID = new PIDConstants(1.0, 0.0, 0.25); public static final PIDConstants ROTATION_PID = new PIDConstants(4.0, 0.0, 0.4); public static final double MAX_MODULE_SPEED = 6.0; diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 20ac055..87b200f 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -18,10 +18,6 @@ public Command intaking() { return new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.INTAKING)); } - // public Command shooting() { - // return new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SHOOTING)); - // } - public Command returnToIdle() { return new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.IDLE)); } From 99c0f4a430e378c41f4a1ad12f5d69aecc85c33e Mon Sep 17 00:00:00 2001 From: Otto Date: Mon, 7 Oct 2024 00:23:17 -0400 Subject: [PATCH 09/12] Nah what bum did this --- src/main/java/frc/robot/subsystems/drive/Drive.java | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e1800d7..180770f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -140,6 +140,7 @@ public void runState() { if (Constants.controller.getBackButtonPressed()) { fieldRelative = !fieldRelative; + Logger.recordOutput("Drive/Field Rel", fieldRelative); } } @@ -188,14 +189,6 @@ public void drive( ); if (headingCorrection) { - // System.out.println(headingCorrection); - // System.out.println("Omgea = 0?" + (omega == 0)); - // System.out.println( - // Math.abs(xSupplier.getAsDouble()) > Constants.Drive.CONTROLLER_DEADBAND - // ); - // System.out.println( - // Math.abs(ySupplier.getAsDouble()) > Constants.Drive.CONTROLLER_DEADBAND - // ); if ( Math.abs(omega) == 0.0 && (Math.abs(xSupplier.getAsDouble()) > Constants.Drive.CONTROLLER_DEADBAND || @@ -206,7 +199,6 @@ public void drive( lastHeadingRadians ) * Constants.Drive.MAX_ANGULAR_SPEED; - // System.out.println("something is happening"); } else { lastHeadingRadians = poseEstimator .getEstimatedPosition() @@ -230,7 +222,6 @@ public void drive( DriverStation.getAlliance().get() == Alliance.Red; if (fieldRelative) { - // TODO: INVERT THE GYRO, MULTIPLYING IT BY 1 IS SO STUPID!!!!!!!!!!!!!!!!!!!!!!!!! drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( linearVelocity.getX() * From 52758d6d571f84c90dcc58dbaca093971c6e221e Mon Sep 17 00:00:00 2001 From: GreenTomato5 Date: Mon, 14 Oct 2024 15:29:38 -0500 Subject: [PATCH 10/12] Clean up stuff --- src/main/java/frc/robot/subsystems/drive/Drive.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 180770f..28032dc 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -83,7 +83,7 @@ public Drive( lastHeadingRadians = poseEstimator.getEstimatedPosition().getRotation().getRadians(); headingCorrectionEnabled = true; - // TODO: Tune + headingCorrectionController = new PIDController( Constants.Drive.HEADING_CORRECTION_PID.kP, Constants.Drive.HEADING_CORRECTION_PID.kI, @@ -119,8 +119,6 @@ public void runState() { // Can't run in auto otherwise it will constantly tell drive not to drive in // auto (and thats not // good) - // Logger.recordOutput("driveState", getState()); - // TODO: TURN ON HEADING CORRECTION LATER if (DriverStation.isTeleop() && getState() != DriveStates.AUTO_ALIGN) { drive( this, From 81d33617961d7d2782483dea8a9157b73f0594ca Mon Sep 17 00:00:00 2001 From: GreenTomato5 Date: Mon, 14 Oct 2024 17:42:17 -0500 Subject: [PATCH 11/12] khsdufudsufhsdjk --- .../frc/robot/subsystems/drive/Drive.java | 28 ++++++++++++++++--- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9a74e36..bfed3cc 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -53,6 +53,7 @@ public class Drive extends Subsystem { private double lastHeadingRadians; private PIDController headingCorrectionController; private boolean headingCorrectionEnabled; + private boolean fieldRelativeEnabled = true; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); @@ -123,18 +124,25 @@ public void runState() { if (DriverStation.isTeleop() && getState() != DriveStates.AUTO_ALIGN) { drive( this, - () -> -Constants.controller.getLeftY(), - () -> -Constants.controller.getLeftX(), + () -> Constants.controller.getLeftY(), + () -> Constants.controller.getLeftX(), () -> Constants.controller.getRightX(), getState().getRotationModifier(), getState().getTranslationModifier(), - headingCorrectionEnabled + headingCorrectionEnabled, + fieldRelativeEnabled ); } if (Constants.controller.getStartButtonPressed()) { zeroGryo(); } + + if (Constants.controller.getBackButtonPressed()) { + fieldRelativeEnabled = !fieldRelativeEnabled; + Logger.recordOutput("Drive", fieldRelativeEnabled); + } + } // L code??? (taken from AA) good enough @@ -164,7 +172,8 @@ public void drive( DoubleSupplier omegaSupplier, double rotationMultiplier, double translationMultiplier, - boolean headingCorrection + boolean headingCorrection, + boolean fieldRelative ) { // Apply deadband double linearMagnitude = MathUtil.applyDeadband( @@ -222,6 +231,7 @@ public void drive( DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( + fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec() * @@ -233,6 +243,15 @@ public void drive( (isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation()).times(-1) + ) : + new ChassisSpeeds( + linearVelocity.getX() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + linearVelocity.getY() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier ) ); } @@ -263,6 +282,7 @@ public void periodic() { module.stop(); } } + // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); From adc649c497cdafa1f9416e7fdf422bde19f506a7 Mon Sep 17 00:00:00 2001 From: github-actions Date: Mon, 14 Oct 2024 23:34:00 +0000 Subject: [PATCH 12/12] Apply Prettier format --- .../frc/robot/subsystems/drive/Drive.java | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9460a8d..24d0748 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -219,28 +219,28 @@ public void drive( DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( - fieldRelative ? - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * - drive.getMaxLinearSpeedMetersPerSec() * - translationMultiplier, - linearVelocity.getY() * - drive.getMaxLinearSpeedMetersPerSec() * - translationMultiplier, - omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, - (isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation()).times(-1) - ) : - new ChassisSpeeds( - linearVelocity.getX() * - drive.getMaxLinearSpeedMetersPerSec() * - translationMultiplier, - linearVelocity.getY() * - drive.getMaxLinearSpeedMetersPerSec() * - translationMultiplier, - omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier - ) + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + linearVelocity.getY() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, + (isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation()).times(-1) + ) + : new ChassisSpeeds( + linearVelocity.getX() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + linearVelocity.getY() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier + ) ); }