diff --git a/src/main/java/team1403/robot/chargedup/AutoManager.java b/src/main/java/team1403/robot/chargedup/AutoManager.java index dfed005..304dd14 100644 --- a/src/main/java/team1403/robot/chargedup/AutoManager.java +++ b/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); diff --git a/src/main/java/team1403/robot/chargedup/CougarRobotImpl.java b/src/main/java/team1403/robot/chargedup/CougarRobotImpl.java index 8f0744a..578858d 100644 --- a/src/main/java/team1403/robot/chargedup/CougarRobotImpl.java +++ b/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 @@ -130,6 +130,7 @@ public void configureDriverInterface() { () -> -deadband(driveController.getRightX(), 0), () -> driveController.getYButton(), () -> 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))); } /** diff --git a/src/main/java/team1403/robot/chargedup/swerve/SwerveCommand.java b/src/main/java/team1403/robot/chargedup/swerve/SwerveCommand.java index 7a63115..9a3c82c 100644 --- a/src/main/java/team1403/robot/chargedup/swerve/SwerveCommand.java +++ b/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 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 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; }