Merge pull request #77 from frc937/swerve-max-speed

Swerve max speed
frc937 · Feb 21, 2024 · ec006a1 · ec006a1
2 parents 73e4f2d + f41ad49
commit ec006a1
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 13 deletions.
diff --git 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
@@ -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<Double> xSupplier, ySupplier, zSupplier;
   private final Supplier<Boolean> upSupplier, downSupplier, leftSupplier, rightSupplier;
 
@@ -52,7 +49,6 @@ public DriveFieldOrientedHeadingSnapping(
       Supplier<Boolean> downSupplier,
       Supplier<Boolean> leftSupplier,
       Supplier<Boolean> 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
@@ -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
@@ -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