From 7fb6e27764d03b03b3447291e53d5ef4209aeacb Mon Sep 17 00:00:00 2001 From: tk2217 Date: Sun, 2 Mar 2025 17:55:22 -0600 Subject: [PATCH 1/6] we use vortexes (#69) --- .../swerve/maxSwerve/modules/backleft.json | 2 +- .../swerve/maxSwerve/modules/backright.json | 2 +- .../swerve/maxSwerve/modules/frontleft.json | 2 +- .../swerve/maxSwerve/modules/frontright.json | 2 +- .../java/frc/robot/commands/drive/Drive.java | 51 +++++++++++++++++++ .../commands/drive/SnapToAngleWithDriver.java | 32 ++++++++++++ 6 files changed, 87 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/commands/drive/Drive.java create mode 100644 src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java diff --git a/src/main/deploy/swerve/maxSwerve/modules/backleft.json b/src/main/deploy/swerve/maxSwerve/modules/backleft.json index c50b06e3..72bc328f 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/backleft.json +++ b/src/main/deploy/swerve/maxSwerve/modules/backleft.json @@ -1,6 +1,6 @@ { "drive": { - "type": "sparkmax_neo", + "type": "sparkmax_vortex", "id": 6, "canbus": null }, diff --git a/src/main/deploy/swerve/maxSwerve/modules/backright.json b/src/main/deploy/swerve/maxSwerve/modules/backright.json index 4bf9b6f8..8f9e03b1 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/backright.json +++ b/src/main/deploy/swerve/maxSwerve/modules/backright.json @@ -1,6 +1,6 @@ { "drive": { - "type": "sparkmax_neo", + "type": "sparkmax_vortex", "id": 8, "canbus": null }, diff --git a/src/main/deploy/swerve/maxSwerve/modules/frontleft.json b/src/main/deploy/swerve/maxSwerve/modules/frontleft.json index 595bd452..be9257f2 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/frontleft.json +++ b/src/main/deploy/swerve/maxSwerve/modules/frontleft.json @@ -1,6 +1,6 @@ { "drive": { - "type": "sparkmax_neo", + "type": "sparkmax_vortex", "id": 2, "canbus": null }, diff --git a/src/main/deploy/swerve/maxSwerve/modules/frontright.json b/src/main/deploy/swerve/maxSwerve/modules/frontright.json index 851a0827..870d43c1 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/frontright.json +++ b/src/main/deploy/swerve/maxSwerve/modules/frontright.json @@ -1,6 +1,6 @@ { "drive": { - "type": "sparkmax_neo", + "type": "sparkmax_vortex", "id": 4, "canbus": null }, diff --git a/src/main/java/frc/robot/commands/drive/Drive.java b/src/main/java/frc/robot/commands/drive/Drive.java new file mode 100644 index 00000000..880be2b6 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/Drive.java @@ -0,0 +1,51 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class Drive extends Command { + + SwerveSubsystem m_drive; + DoubleSupplier m_x; + DoubleSupplier m_y; + DoubleSupplier m_rotation; + BooleanSupplier m_slow; + Optional m_throttle; + + public Drive( + SwerveSubsystem drive, + DoubleSupplier x, + DoubleSupplier y, + DoubleSupplier rotation, + BooleanSupplier slow, + Optional throttle) { + m_drive = drive; + m_x = x; + m_y = y; + m_rotation = rotation; + m_slow = slow; + m_throttle = throttle; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java b/src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java new file mode 100644 index 00000000..a883bf7a --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java @@ -0,0 +1,32 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; + +public class SnapToAngleWithDriver extends Command { + + SwerveSubsystem m_drive; + + public SnapToAngleWithDriver(SwerveSubsystem drive) { + m_drive = drive; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} From 32774b15d9bcabb7795f2650af5068e66293945b Mon Sep 17 00:00:00 2001 From: samfreund Date: Mon, 3 Mar 2025 16:25:04 -0600 Subject: [PATCH 2/6] rewrote to add drive command --- src/main/java/frc/robot/Constants.java | 4 ++ src/main/java/frc/robot/RobotContainer.java | 24 +++++----- .../java/frc/robot/commands/drive/Drive.java | 47 +++++++++++++++---- 3 files changed, 54 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c882d8a5..2a4e695b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -367,5 +367,9 @@ public static final class DriveConstants { // the robot, rather the allowed maximum speeds public static final double kMaxSpeedMetersPerSecond = 4.8; public static final double kMaxAngularSpeed = kTau; // radians per second + + // First one is normal, second is slow + public static final double[] kRotationSpeeds = {0.7, 0.3}; + public static final double[] kDrivingSpeeds = {0.5, 0.3}; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 954119c4..8cdf4e87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.OIConstants; +import frc.robot.commands.drive.Drive; import frc.robot.subsystems.CoralHandlerSubsystem; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.sim.CoralHandlerSubsystemSim; @@ -18,7 +19,7 @@ import frc.robot.utils.FilteredButton; import frc.robot.utils.FilteredJoystick; import java.io.File; -import swervelib.SwerveInputStream; +import java.util.Optional; /* * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -49,23 +50,20 @@ public class RobotContainer { // Button Board private final FilteredButton m_buttonBoard = new FilteredButton(OIConstants.kButtonBoardPort); - // Configure drive input stream - SwerveInputStream driveInput = - SwerveInputStream.of( - m_drive.getSwerveDrive(), - m_operatorController::getLeftY, - m_operatorController::getLeftX) - .withControllerRotationAxis(() -> -m_operatorController.getRightX()) - .deadband(0.1) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); // Set default drive command - m_drive.setDefaultCommand(m_drive.driveFieldOriented(driveInput)); + + m_drive.setDefaultCommand( + new Drive( + m_drive, + m_operatorController::getRightX, + m_operatorController::getLeftY, + () -> -m_operatorController.getRightX(), + () -> m_operatorController.rightBumper().getAsBoolean(), + Optional.empty())); } /** diff --git a/src/main/java/frc/robot/commands/drive/Drive.java b/src/main/java/frc/robot/commands/drive/Drive.java index 880be2b6..ac54f16b 100644 --- a/src/main/java/frc/robot/commands/drive/Drive.java +++ b/src/main/java/frc/robot/commands/drive/Drive.java @@ -1,19 +1,22 @@ package frc.robot.commands.drive; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import swervelib.SwerveInputStream; public class Drive extends Command { - SwerveSubsystem m_drive; - DoubleSupplier m_x; - DoubleSupplier m_y; - DoubleSupplier m_rotation; - BooleanSupplier m_slow; - Optional m_throttle; + final SwerveSubsystem m_drive; + final DoubleSupplier m_x; + final DoubleSupplier m_y; + final DoubleSupplier m_rotation; + final BooleanSupplier m_slow; + final Optional m_throttle; + SwerveInputStream driveInput; public Drive( SwerveSubsystem drive, @@ -33,11 +36,39 @@ public Drive( // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + driveInput = + SwerveInputStream.of( + m_drive.getSwerveDrive(), + () -> + m_x.getAsDouble() + * (m_slow.getAsBoolean() + ? DriveConstants.kDrivingSpeeds[1] + : DriveConstants.kDrivingSpeeds[0]) + * m_throttle.orElse(() -> 1.0).getAsDouble(), + () -> + m_y.getAsDouble() + * (m_slow.getAsBoolean() + ? DriveConstants.kDrivingSpeeds[1] + : DriveConstants.kDrivingSpeeds[0]) + * m_throttle.orElse(() -> 1.0).getAsDouble()) + .withControllerRotationAxis( + () -> + m_rotation.getAsDouble() + * (m_slow.getAsBoolean() + ? DriveConstants.kRotationSpeeds[1] + : DriveConstants.kRotationSpeeds[0]) + * m_throttle.orElse(() -> 1.0).getAsDouble()) + .deadband(0.1) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + m_drive.driveFieldOriented(driveInput); + } // Called once the command ends or is interrupted. @Override From 3e2a5a46686290a9335d591cdd6054a53bdfe9e7 Mon Sep 17 00:00:00 2001 From: samfreund Date: Tue, 11 Mar 2025 14:06:29 -0500 Subject: [PATCH 3/6] initial --- src/main/java/frc/robot/RobotContainer.java | 5 ++ .../commands/drive/SnapToAngleWithDriver.java | 48 +++++++++++++++++-- 2 files changed, 49 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8cdf4e87..84f1256e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,8 +9,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OIConstants; import frc.robot.commands.drive.Drive; +import frc.robot.commands.drive.SnapToAngleWithDriver; import frc.robot.subsystems.CoralHandlerSubsystem; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.sim.CoralHandlerSubsystemSim; @@ -82,6 +84,9 @@ private void configureButtonBindings() { .onTrue(Commands.runOnce(() -> ((CoralHandlerSubsystemSim) m_coral).getSimCoral())); } + new Trigger(m_driverRightJoystick::getPOVPressed) + .onTrue(new SnapToAngleWithDriver(m_drive, null, null, null, null)); + m_operatorController .x() .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2))); diff --git a/src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java b/src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java index a883bf7a..7b53dc73 100644 --- a/src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java +++ b/src/main/java/frc/robot/commands/drive/SnapToAngleWithDriver.java @@ -1,24 +1,64 @@ package frc.robot.commands.drive; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import swervelib.SwerveInputStream; public class SnapToAngleWithDriver extends Command { - SwerveSubsystem m_drive; + final SwerveSubsystem m_drive; + final DoubleSupplier m_x; + final DoubleSupplier m_y; + final BooleanSupplier m_slow; + final Optional m_throttle; + SwerveInputStream driveInput; - public SnapToAngleWithDriver(SwerveSubsystem drive) { + public SnapToAngleWithDriver( + SwerveSubsystem drive, + DoubleSupplier x, + DoubleSupplier y, + BooleanSupplier slow, + Optional throttle) { m_drive = drive; + m_x = x; + m_y = y; + m_slow = slow; + m_throttle = throttle; addRequirements(m_drive); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + driveInput = + SwerveInputStream.of( + m_drive.getSwerveDrive(), + () -> + m_x.getAsDouble() + * (m_slow.getAsBoolean() + ? DriveConstants.kDrivingSpeeds[1] + : DriveConstants.kDrivingSpeeds[0]) + * m_throttle.orElse(() -> 1.0).getAsDouble(), + () -> + m_y.getAsDouble() + * (m_slow.getAsBoolean() + ? DriveConstants.kDrivingSpeeds[1] + : DriveConstants.kDrivingSpeeds[0]) + * m_throttle.orElse(() -> 1.0).getAsDouble()) + .deadband(0.1) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + m_drive.driveFieldOriented(driveInput); + } // Called once the command ends or is interrupted. @Override From bc1dad5b90f2a92270f35e040f6aafdc7fb83d5a Mon Sep 17 00:00:00 2001 From: samfreund Date: Tue, 11 Mar 2025 14:09:35 -0500 Subject: [PATCH 4/6] lint --- src/main/java/frc/robot/RobotContainer.java | 43 ++++++++++----------- 1 file changed, 21 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ac6b04b1..1708f151 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,13 +9,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.OIConstants; -import frc.robot.commands.drive.Drive; -import frc.robot.commands.drive.SnapToAngleWithDriver; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IOConstants; import frc.robot.commands.drive.Drive; +import frc.robot.commands.drive.SnapToAngleWithDriver; import frc.robot.subsystems.CoralHandlerSubsystem; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.sim.CoralHandlerSubsystemSim; @@ -112,24 +110,25 @@ private void configureButtonBindings() { .onTrue( Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); - new Trigger(m_driverRightJoystick::getPOVPressed) - .onTrue(new SnapToAngleWithDriver(m_drive, null, null, null, null)); - - m_operatorController - .x() - .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2))); - m_operatorController - .y() - .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); - - m_operatorController - .rightBumper() - .onTrue(Commands.runOnce(m_coral::grab)) - .onFalse(Commands.runOnce(m_coral::idle)); - m_operatorController - .leftBumper() - .onTrue(Commands.runOnce(m_coral::release)) - .onFalse(Commands.runOnce(m_coral::idle)); + new Trigger(m_driverRightJoystick::getPOVPressed) + .onTrue(new SnapToAngleWithDriver(m_drive, null, null, null, null)); + + m_operatorController + .x() + .onTrue(Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.L2))); + m_operatorController + .y() + .onTrue( + Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); + + m_operatorController + .rightBumper() + .onTrue(Commands.runOnce(m_coral::grab)) + .onFalse(Commands.runOnce(m_coral::idle)); + m_operatorController + .leftBumper() + .onTrue(Commands.runOnce(m_coral::release)) + .onFalse(Commands.runOnce(m_coral::idle)); m_operatorController .rightBumper() .onTrue(Commands.runOnce(m_coral::grab)) From 939d9dcebfd037242cc5ebf09fa31ea3e19ed263 Mon Sep 17 00:00:00 2001 From: samfreund Date: Tue, 11 Mar 2025 20:01:20 -0500 Subject: [PATCH 5/6] linting --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a442306c..163b720b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -129,7 +129,6 @@ private void configureButtonBindings() { .leftBumper() .onTrue(Commands.runOnce(m_coral::release)) .onFalse(Commands.runOnce(m_coral::idle)); - m_controller m_controller .rightBumper() .onTrue(Commands.runOnce(m_coral::grab)) From 56a5888a133d931cfed9152be8c04eaaca210010 Mon Sep 17 00:00:00 2001 From: samfreund Date: Fri, 14 Mar 2025 21:26:56 -0500 Subject: [PATCH 6/6] this is what you get for not having checks --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e529c3e9..ab5038c7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -130,7 +130,8 @@ private void configureButtonBindings() { .onTrue( Commands.runOnce(() -> m_elevator.setState(ElevatorSubsystem.ElevatorState.DOWN))); - new Trigger(m_rightJoystick::getPOVPressed) + m_rightJoystick + .getPOVPressed() .onTrue(new SnapToAngleWithDriver(m_drive, null, null, null, null)); m_controller