diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a19b1e1..f3d84cf 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -11,7 +11,7 @@ on: concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.ref }} - cancel-in-progress: true + cancel-in-progress: false # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: diff --git a/.github/workflows/codeql.yml b/.github/workflows/codeql.yml index 2b407ec..2d18172 100644 --- a/.github/workflows/codeql.yml +++ b/.github/workflows/codeql.yml @@ -11,7 +11,7 @@ on: concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.ref }} - cancel-in-progress: true + cancel-in-progress: false jobs: CodeQL-Build: diff --git a/.github/workflows/gradle-wrapper-validation.yml b/.github/workflows/gradle-wrapper-validation.yml index d640023..82b95c9 100644 --- a/.github/workflows/gradle-wrapper-validation.yml +++ b/.github/workflows/gradle-wrapper-validation.yml @@ -11,7 +11,7 @@ on: concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.ref }} - cancel-in-progress: true + cancel-in-progress: false # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: diff --git a/.github/workflows/qodana.yml b/.github/workflows/qodana.yml index b9ba5e8..af018b0 100644 --- a/.github/workflows/qodana.yml +++ b/.github/workflows/qodana.yml @@ -13,7 +13,7 @@ on: concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.ref }} - cancel-in-progress: true + cancel-in-progress: false jobs: qodana: diff --git a/.github/workflows/syntax-check.yml b/.github/workflows/syntax-check.yml index 0c470f4..3fe4bad 100644 --- a/.github/workflows/syntax-check.yml +++ b/.github/workflows/syntax-check.yml @@ -15,7 +15,7 @@ permissions: concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.ref }} - cancel-in-progress: true + cancel-in-progress: false # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 531351e..7e1e0b8 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -7,7 +7,10 @@ "Blue Station 2", "Blue Station 3" ], - "autoFolders": [], + "autoFolders": [ + "Red", + "Blue" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5629dcd..ff2e7e7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -119,6 +119,7 @@ public static class Deadbands { public static final double LEFT_X = 0.1; public static final double LEFT_Y = 0.1; public static final double RIGHT_X = 0.1; + public static final double RIGHT_Y = 0.1; public static final double TURN_CONSTANT = 6; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6df93c5..1e917bd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,8 @@ import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.OperatorConstants; +import frc.robot.Constants.OperatorConstants.Deadbands; +import frc.robot.commands.swerve.AbsoluteDrive; import frc.robot.subsystems.ConveyorSubsystem; import frc.robot.subsystems.DumpSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -28,6 +29,11 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { + // Controllers and joysticks are defined here + final CommandXboxController driverXbox = + new CommandXboxController(Constants.OperatorConstants.Joysticks.Port.DRIVER_CONTROLLER); + final CommandJoystick copilotJoystick = + new CommandJoystick(Constants.OperatorConstants.Joysticks.Port.COPILOT_CONTROLLER); // The robot's subsystems and commands are defined here... private final SwerveSubsystem drivebase = @@ -36,42 +42,28 @@ public class RobotContainer { private final ConveyorSubsystem conveyor = new ConveyorSubsystem(); private final DumpSubsystem dump = new DumpSubsystem(); - // Controllers and joysticks are defined here - final CommandXboxController driverXbox = - new CommandXboxController(Constants.OperatorConstants.Joysticks.Port.DRIVER_CONTROLLER); - final CommandJoystick copilotJoystick = - new CommandJoystick(Constants.OperatorConstants.Joysticks.Port.COPILOT_CONTROLLER); + // Apply deadbands to the joysticks + double driverLeftX = MathUtil.applyDeadband(driverXbox.getLeftX(), Deadbands.LEFT_X); + double driverLeftY = MathUtil.applyDeadband(driverXbox.getLeftY(), Deadbands.LEFT_Y); + double driverRightX = MathUtil.applyDeadband(driverXbox.getRightX(), Deadbands.RIGHT_X); + double driverRightY = MathUtil.applyDeadband(driverXbox.getRightY(), Deadbands.RIGHT_Y); + + private final AbsoluteDrive absoluteDrive = + new AbsoluteDrive( + drivebase, + () -> driverLeftX, + () -> driverLeftY, + () -> driverRightX, + () -> driverRightY + ); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings configureBindings(); - // Applies deadbands and inverts controls because joysticks - // are back-right positive while robot - // controls are front-left positive - // left stick controls translation - // right stick controls the desired angle NOT angular rotation - Command driveFieldOrientedDirectAngle = - drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.Deadbands.LEFT_Y), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.Deadbands.LEFT_X), - () -> driverXbox.getRightX(), - () -> driverXbox.getRightY()); - - // Applies deadbands and inverts controls because joysticks - // are back-right positive while robot - // controls are front-left positive - // left stick controls translation - // right stick controls the angular velocity of the robot - Command driveFieldOrientedAnglularVelocity = - drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.Deadbands.LEFT_Y), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.Deadbands.LEFT_X), - () -> driverXbox.getRightX() * 0.5); - drivebase.setMotorBrake(true); - drivebase.setDefaultCommand(driveFieldOrientedDirectAngle); + drivebase.setDefaultCommand(absoluteDrive); } /** @@ -95,12 +87,12 @@ private void configureBindings() { new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))))); driverXbox.y().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - copilotJoystick.trigger().onTrue(Commands.runOnce(dump::extend)); - copilotJoystick.trigger().onFalse(Commands.runOnce(dump::retract)); - copilotJoystick.button(3).whileTrue(Commands.runOnce(intake::runIntake)); - copilotJoystick.button(4).whileTrue(Commands.runOnce(intake::reverseIntake)); - copilotJoystick.button(5).whileTrue(Commands.runOnce(conveyor::runConveyor)); - copilotJoystick.button(6).whileTrue(Commands.runOnce(conveyor::reverseConveyor)); + copilotJoystick.trigger().onTrue(Commands.runOnce(dump::extend).repeatedly()); + copilotJoystick.trigger().onFalse(Commands.runOnce(dump::retract).repeatedly()); + copilotJoystick.button(3).whileTrue(Commands.runOnce(intake::runIntake).repeatedly()); + copilotJoystick.button(4).whileTrue(Commands.runOnce(intake::reverseIntake).repeatedly()); + copilotJoystick.button(5).whileTrue(Commands.runOnce(conveyor::runConveyor).repeatedly()); + copilotJoystick.button(6).whileTrue(Commands.runOnce(conveyor::reverseConveyor).repeatedly()); } /**