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); diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java index 0fef6659..ab58d5f3 100644 --- a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java +++ b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java @@ -13,9 +13,7 @@ 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.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; import java.util.function.Supplier; @@ -27,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; @@ -52,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; @@ -93,7 +89,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); } 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); 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