From 6b41a1a35c96a7f39979e40ba7bd933d38ffd284 Mon Sep 17 00:00:00 2001 From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com> Date: Tue, 20 Feb 2024 16:44:02 -0600 Subject: [PATCH 1/5] Add maximum speed and maximum angular speed methods. --- src/main/java/frc/robot/subsystems/Drive.java | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 60772b45..d3f3267e 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -110,6 +110,24 @@ public void enterXMode() { drive.lockPose(); } + /** + * Gets the maximum speed the robot chassis can acheive in m/s. + * + * @return Maximum speed the robot chassis can acheive in m/s. + */ + public double getMaximumSpeed() { + return drive.getMaximumVelocity(); + } + + /** + * Gets the maximum angular speed the robot chassis can acheive in rad/s. + * + * @return Maximum angular speed the robot chassis can acheive in rad/s. + */ + public double getMaximumAngularSpeed() { + return drive.getMaximumAngularVelocity(); + } + /** * Set the heading correction capabilities of YAGSL. Should only be enabled when heading * correction capabilities are in use From 2e31dc7f9417eb3554c318b9b5db9bbe9aa258ab Mon Sep 17 00:00:00 2001 From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com> Date: Tue, 20 Feb 2024 16:54:42 -0600 Subject: [PATCH 2/5] Update DriveFieldOriented to use correct constants --- .../java/frc/robot/commands/drive/DriveFieldOriented.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java b/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java index ea4b2e3f..35a384fe 100644 --- a/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java +++ b/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; import java.util.function.Supplier; @@ -46,9 +45,9 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double x = this.xSupplier.get() * Constants.Drive.MAX_SPEED; - double y = this.ySupplier.get() * Constants.Drive.MAX_SPEED; - double z = this.zSupplier.get() * Constants.Drive.MAX_ANGULAR_SPEED; + double x = this.xSupplier.get() * drive.getMaximumSpeed(); + double y = this.ySupplier.get() * drive.getMaximumSpeed(); + double z = this.zSupplier.get() * drive.getMaximumAngularSpeed(); Translation2d translation = new Translation2d(x, y); drive.driveFieldOriented(translation, z); From 5f93e0fdd6a9eb7d944c0f6f77a103dbcdd03619 Mon Sep 17 00:00:00 2001 From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com> Date: Tue, 20 Feb 2024 16:59:34 -0600 Subject: [PATCH 3/5] Fix DriveFieldOrientedHeadingSnapping --- .../commands/drive/DriveFieldOrientedHeadingSnapping.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java index 25589a76..b3a2b081 100644 --- a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java +++ b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; import java.util.function.Supplier; @@ -93,7 +92,7 @@ public void execute() { Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); if (headingX == 0 && headingY == 0 && Math.abs(zSupplier.get()) > 0) { - drive.driveFieldOriented(translation, zSupplier.get() * Constants.Drive.MAX_ANGULAR_SPEED); + drive.driveFieldOriented(translation, zSupplier.get() * drive.getMaximumAngularSpeed()); } else { drive.driveFieldOriented(translation, desiredSpeeds.omegaRadiansPerSecond); } From 49f3f662c997bd00a84cdf8b835f0c0bfcab9d0e Mon Sep 17 00:00:00 2001 From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com> Date: Tue, 20 Feb 2024 17:00:07 -0600 Subject: [PATCH 4/5] Remove unused XboxController --- .../commands/drive/DriveFieldOrientedHeadingSnapping.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java index b3a2b081..be3c9ef5 100644 --- a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java +++ b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; @@ -26,7 +25,6 @@ */ public class DriveFieldOrientedHeadingSnapping extends Command { private Drive drive; - private XboxController controller; private final Supplier xSupplier, ySupplier, zSupplier; private final Supplier upSupplier, downSupplier, leftSupplier, rightSupplier; @@ -51,7 +49,6 @@ public DriveFieldOrientedHeadingSnapping( Supplier downSupplier, Supplier leftSupplier, Supplier rightSupplier) { - this.controller = RobotContainer.driverController.getHID(); this.drive = RobotContainer.drive; this.xSupplier = xSupplier; this.ySupplier = ySupplier; From d02a11755ae98ea4230e290ca6b69fb57cd2ef82 Mon Sep 17 00:00:00 2001 From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com> Date: Tue, 20 Feb 2024 17:01:25 -0600 Subject: [PATCH 5/5] Fix DriveRobotOriented --- .../java/frc/robot/commands/drive/DriveRobotOriented.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java b/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java index 41e141fc..e9b5c02e 100644 --- a/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java +++ b/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; import java.util.function.Supplier; @@ -46,9 +45,9 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double x = xSupplier.get() * Constants.Drive.MAX_SPEED; - double y = ySupplier.get() * Constants.Drive.MAX_SPEED; - double z = zSupplier.get() * Constants.Drive.MAX_ANGULAR_SPEED; + double x = xSupplier.get() * drive.getMaximumSpeed(); + double y = ySupplier.get() * drive.getMaximumSpeed(); + double z = zSupplier.get() * drive.getMaximumAngularSpeed(); Translation2d translation = new Translation2d(x, y); drive.driveRobotOriented(translation, z);