fix auto speed limiter a different way

Author: Etaash-mathamsetty

src/main/java/team1403/robot/chargedup/AutoManager.java

@@ -76,7 +76,8 @@ public void init(SwerveSubsystem swerve, ArmSubsystem arm) {
     pathplannerAuto = autoBuilder.fullAuto(pathGroup).andThen(() -> swerve.stop(), swerve);
     twoPieceAuto = autoBuilder.fullAuto(twoPiece).andThen(() -> swerve.stop(), swerve);
     threePieceAuto = autoBuilder.fullAuto(threePiece).andThen(() -> swerve.stop(), swerve);
-  } 
+  }
+  
   public CommandBase getThreePieceAuto(SwerveSubsystem swerve)
   {
     swerve.setSpeedLimiter(1.0);

src/main/java/team1403/robot/chargedup/CougarRobotImpl.java

@@ -94,7 +94,7 @@ public Command getAutonomousCommand() {
     CommandScheduler.getInstance().removeDefaultCommand(m_swerveSubsystem);
     CommandScheduler.getInstance().removeDefaultCommand(m_arm);
     //force the swerve subsystem to stop running the default command, setting the speed limiter should now work
-    new InstantCommand(() -> m_swerveSubsystem.stop(), m_swerveSubsystem);
+    //new InstantCommand(() -> m_swerveSubsystem.stop(), m_swerveSubsystem);
     return m_autonChooser.getSelected();
 
     // return
@@ -129,7 +129,8 @@ public void configureDriverInterface() {
         () -> -deadband(driveController.getLeftY(), 0),
         () -> -deadband(driveController.getRightX(), 0),
         () -> driveController.getYButton(),
-        () -> driveController.getRightTriggerAxis()
+        () -> driveController.getRightTriggerAxis(),
+        () -> getMode()
         ));
 
     new Trigger(() -> driveController.getBButton()).onFalse(
@@ -143,10 +144,6 @@ public void configureDriverInterface() {
         .onFalse(new InstantCommand(() -> m_swerveSubsystem.setXModeEnabled(false)));
     new Trigger(() -> driveController.getPOV() == 180)
         .toggleOnTrue(new InstantCommand(() -> m_swerveSubsystem.resetOdometry()));
-    new Trigger(() -> driveController.getRightBumperPressed())
-        .toggleOnTrue(new InstantCommand(() -> m_swerveSubsystem.increaseSpeed(0.2)));
-    new Trigger(() -> driveController.getLeftBumperPressed())
-        .toggleOnTrue(new InstantCommand(() -> m_swerveSubsystem.decreaseSpeed(0.2)));
   }
 
   /**

src/main/java/team1403/robot/chargedup/swerve/SwerveCommand.java

@@ -2,12 +2,14 @@
 
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import team1403.lib.core.CougarRobot;
 import team1403.robot.chargedup.AutoManager;
 import team1403.robot.chargedup.RobotConfig;
 import team1403.robot.chargedup.RobotConfig.Swerve;
@@ -23,6 +25,7 @@ public class SwerveCommand extends CommandBase {
   private final DoubleSupplier m_rotationSupplier;
   private final BooleanSupplier m_fieldRelativeSupplier;
   private final DoubleSupplier m_speedDoubleSupplier;
+  private final Supplier<CougarRobot.Mode> m_modeSupplier;
   private boolean m_isFieldRelative;
 
   private Translation2d frontRight;
@@ -56,13 +59,15 @@ public SwerveCommand(SwerveSubsystem drivetrain,
       DoubleSupplier verticalTranslationSupplier,
       DoubleSupplier rotationSupplier,
       BooleanSupplier fieldRelativeSupplier,
-      DoubleSupplier speedDoubleSupplier) {
+      DoubleSupplier speedDoubleSupplier,
+      Supplier<CougarRobot.Mode> modeSupplier) {
     this.m_drivetrainSubsystem = drivetrain;
     this.m_verticalTranslationSupplier = verticalTranslationSupplier;
     this.m_horizontalTranslationSupplier = horizontalTranslationSupplier;
     this.m_rotationSupplier = rotationSupplier;
     this.m_fieldRelativeSupplier = fieldRelativeSupplier;
     this.m_speedDoubleSupplier = speedDoubleSupplier;
+    this.m_modeSupplier = modeSupplier;
     m_isFieldRelative = true;
 
     frontRight = new Translation2d(
@@ -89,7 +94,14 @@ public SwerveCommand(SwerveSubsystem drivetrain,
 
   @Override
   public void execute() {
-    m_drivetrainSubsystem.setSpeedLimiter(0.2 + (m_speedDoubleSupplier.getAsDouble() * 0.8));
+    if(m_modeSupplier.get() == CougarRobot.Mode.TELEOP)
+    {
+      m_drivetrainSubsystem.setSpeedLimiter(0.2 + (m_speedDoubleSupplier.getAsDouble() * 0.8));
+    }
+    else if(m_modeSupplier.get() == CougarRobot.Mode.AUTONOMOUS)
+    {
+      m_drivetrainSubsystem.setSpeedLimiter(1.0);
+    }
     if (m_fieldRelativeSupplier.getAsBoolean()) {
       m_isFieldRelative = !m_isFieldRelative;
     }