From 7451ce375f63428162d4149b02d24fd0f7a30801 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Mon, 29 Jul 2024 18:40:03 -0500 Subject: [PATCH] Scale the input by 80% power Signed-off-by: thenetworkgrinch --- .../frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 923149f4..cdc06acd 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -252,8 +252,8 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. return run(() -> { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(translationX.getAsDouble(), - translationY.getAsDouble())); + Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(), + translationY.getAsDouble()), 0.8); // Make the robot move driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(), @@ -325,9 +325,9 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat { return run(() -> { // Make the robot move - swerveDrive.drive(SwerveMath.cubeTranslation(new Translation2d( + swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d( translationX.getAsDouble() * swerveDrive.getMaximumVelocity(), - translationY.getAsDouble() * swerveDrive.getMaximumVelocity())), + translationY.getAsDouble() * swerveDrive.getMaximumVelocity()), 0.8), Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), true, false);