From aef72a69ead4f81fe8bdce888ec01a05da759285 Mon Sep 17 00:00:00 2001 From: willitcode <91231142+willitcode@users.noreply.github.com> Date: Thu, 4 Apr 2024 09:29:05 -0500 Subject: [PATCH 01/53] theoretically fix limelight aiming --- src/main/java/frc/robot/commands/AimWithLimelight.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java index c04d3c8..ebd14ec 100644 --- a/src/main/java/frc/robot/commands/AimWithLimelight.java +++ b/src/main/java/frc/robot/commands/AimWithLimelight.java @@ -157,7 +157,7 @@ public void execute() { */ hasSeenTarget = true; - drive.driveRobot(new Translation2d(getX() * -1.0, 0.0), getRotation(), false); + drive.driveRobot(new Translation2d(getX() * -1.0, 0.0), -1 * getRotation(), false); /* End the command if we're at our "aimed" threshold */ if (isAngled() && isDistanced()) { From 44790f927a221e1ffd602540efd58207bb7a81a5 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Thu, 4 Apr 2024 10:51:38 -0500 Subject: [PATCH 02/53] WHY ON EARTH WERE RELAY IDS BROKEN --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fb87134..2c3f8e6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,7 +39,7 @@ public static final class Mailbox { /** Constants for the Pneumatics system. */ public static final class MailboxPneumatics { /** The ID for the relay that controls the mailbox's solenoids */ - public static final int MAILBOX_SOLENOID_RELAY_ID = 0; + public static final int MAILBOX_SOLENOID_RELAY_ID = 1; } /** Constants for the Belts system. */ From ccff581cda6f15d2a235bb33adb744c0c790340f Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Thu, 4 Apr 2024 14:25:40 -0500 Subject: [PATCH 03/53] Add auto to chooser --- src/main/java/frc/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9258b5d..ac56a59 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -328,6 +328,8 @@ private void configureAuto() { /* This is where you put auto commands. Call autoChooser.addOption() to add autos. */ autoChooser.addOption("Taxi", taxiAuto); + autoChooser.addOption("One Note With Limelight", onePieceAuto); + Shuffleboard.getTab("Driver").add("Choose Auto Routine", autoChooser); } From 85d695cfbe0c7dcc4affcf87948d80cc026affbe Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Thu, 4 Apr 2024 14:25:55 -0500 Subject: [PATCH 04/53] LL angle and height --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2c3f8e6..342c647 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -161,10 +161,10 @@ public static final class AimingLimelight { public static final String LIMELIGHT_NAME = "limelight"; /** The number of degrees the Limelight is mounted back from perfectly vertical */ - public static final double MOUNT_ANGLE = 50; + public static final double MOUNT_ANGLE = 45; /** The number of inches from the center of the Limelight lens to the floor */ - public static final double MOUNT_HEIGHT = 10.0625; + public static final double MOUNT_HEIGHT = 19.5; /** The height to the Amp Apriltag off the ground. */ public static final double AMP_APRILTAG_HEIGHT = 53.13; From 092ddbc952b6fef8761be476dbb4bb1c23ea2718 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Thu, 4 Apr 2024 14:26:06 -0500 Subject: [PATCH 05/53] heading snapping sprint quickfix --- src/main/java/frc/robot/Controllers.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../SetDrivePerspectiveFieldOrientedHeadingSnapping.java | 6 ++++-- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index 647b395..76e7b90 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -103,7 +103,7 @@ private static void configureDefaultKeybinds() { operatorController.povDown().whileTrue(RobotContainer.runIntakeReverse); operatorController.leftTrigger().whileTrue(RobotContainer.aimToAmp); operatorController.rightTrigger().whileTrue(RobotContainer.fireNote); - pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint); + pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedHeadingSnapping); /* TODO: angle / velocity steering toggle w/ right trigger (no issue) */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ac56a59..16a3159 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -310,7 +310,7 @@ public RobotContainer() { startIntakeCamera.schedule(); - drive.setDefaultCommand(driveFieldOrientedHeadingSnapping); + drive.setDefaultCommand(driveFieldOriented); robotLights.setDefaultCommand(enabledLights); compressor.setDefaultCommand(controlCompressor); } diff --git a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java index 61faf25..f5d79c5 100644 --- a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java +++ b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java @@ -29,8 +29,10 @@ public SetDrivePerspectiveFieldOrientedHeadingSnapping() { // Called when the command is initially scheduled. @Override public void initialize() { - drive.setDefaultCommand(RobotContainer.driveFieldOrientedHeadingSnapping); - Controllers.pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint); + drive.setDefaultCommand(RobotContainer.driveFieldOriented); + Controllers.pilotController + .leftBumper() + .whileTrue(RobotContainer.driveFieldOrientedHeadingSnapping); } // Called every time the scheduler runs while the command is scheduled. From c91457bbf72b62063cc878e6afe858d1fb616350 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Thu, 4 Apr 2024 16:58:47 -0500 Subject: [PATCH 06/53] shorten compressor deadband --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 342c647..4a5a541 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -273,6 +273,6 @@ public static final class Compressor { * How many periodic loops do we want to wait for to turn off the compressor. Used to prevent * rapidly turning on and off the compressor. */ - public static final int COMPRESSOR_PRESSURE_SWITCH_DEADBAND = 100; /* = 2 Seconds */ + public static final int COMPRESSOR_PRESSURE_SWITCH_DEADBAND = 50; /* 1 second */ } } From 745ef5d45e393c9259005b0a87d88101623bf7f6 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Thu, 4 Apr 2024 16:59:31 -0500 Subject: [PATCH 07/53] move belt follower call technically unnecessary but w/e Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java b/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java index 826bc09..c75d03e 100644 --- a/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java +++ b/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java @@ -32,11 +32,12 @@ public MailboxBelts() { lowerBeltMotor.setSmartCurrentLimit(Constants.MailboxBelts.BELT_MOTOR_CURRENT_LIMIT); upperBeltMotor.setInverted(Constants.MailboxBelts.UPPER_BELT_MOTOR_INVERTED); - lowerBeltMotor.follow(upperBeltMotor, Constants.MailboxBelts.BELTS_FOLLOWER_INVERSE_STATE); upperBeltMotor.setIdleMode(Constants.MailboxBelts.BELTS_IDLE_MODE); lowerBeltMotor.setIdleMode(Constants.MailboxBelts.BELTS_IDLE_MODE); + lowerBeltMotor.follow(upperBeltMotor, Constants.MailboxBelts.BELTS_FOLLOWER_INVERSE_STATE); + upperBeltMotor.burnFlash(); lowerBeltMotor.burnFlash(); } From 7357a64eff398ff5f58d42ba0b0d2f4b9dc1b7fd Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 08:27:17 -0500 Subject: [PATCH 08/53] limelight theoretically works I hope Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- .../frc/robot/commands/AimWithLimelight.java | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java index ebd14ec..f7ab344 100644 --- a/src/main/java/frc/robot/commands/AimWithLimelight.java +++ b/src/main/java/frc/robot/commands/AimWithLimelight.java @@ -12,19 +12,21 @@ package frc.robot.commands; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; import frc.robot.subsystems.Limelight; +import swervelib.SwerveController; /** Aims with the limelight towards an object. */ public class AimWithLimelight extends Command { private Drive drive; private Limelight limelight; - private boolean finished, hasSeenTarget; - private int counter; - + private boolean finished, hasSeenTarget, isBlueAlliance; + private int counter, snapAngleXComponent; private double steerStrength, desiredDistanceFromTarget, mountHeight, @@ -142,9 +144,17 @@ private boolean isDistanced() { public void initialize() { this.oldPipelineNumber = limelight.getLimelightPipeline(); limelight.setLimelightPipeline(pipelineNumber); + drive.setHeadingCorrection(true); this.finished = false; this.hasSeenTarget = false; this.counter = 0; + + this.snapAngleXComponent = -1; + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false) { + this.snapAngleXComponent *= -1; + } } // Called every time the scheduler runs while the command is scheduled. @@ -155,9 +165,13 @@ public void execute() { /* Remember if we've seen a target so that we can assume we're aimed if we lose sight of the target * Only reason we assume this is because of where the Limelight is, which results in us not actually seeing the target if we're properly aimed */ - hasSeenTarget = true; + // hasSeenTarget = true; + + ChassisSpeeds desiredSpeeds = + drive.getTargetSpeeds(getX(), getRotation(), snapAngleXComponent, 0); - drive.driveRobot(new Translation2d(getX() * -1.0, 0.0), -1 * getRotation(), false); + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + drive.driveRobot(translation, desiredSpeeds.omegaRadiansPerSecond, false); /* End the command if we're at our "aimed" threshold */ if (isAngled() && isDistanced()) { From 0caf91d12bc82b750b0e165ce24e508d2cb48503 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 08:27:38 -0500 Subject: [PATCH 09/53] don't seek in auto Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- .../java/frc/robot/commands/auto/OnePieceAuto.java | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAuto.java b/src/main/java/frc/robot/commands/auto/OnePieceAuto.java index 406c1b5..01b653b 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAuto.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAuto.java @@ -12,10 +12,7 @@ package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants; -import frc.robot.RobotContainer; import frc.robot.commands.AimAndFireRoutine; -import frc.robot.commands.SeekTargetWithLimelight; /* Enter weeb joke here. */ /** @@ -27,11 +24,10 @@ public class OnePieceAuto extends SequentialCommandGroup { /** Creates a new OnePieceAuto. */ public OnePieceAuto() { addCommands( - new SeekTargetWithLimelight( - RobotContainer.limelight, - Constants.Limelight.AimingLimelight.PipelineNumbers.AMP_PIPELINE_NUMBER, - Constants.Limelight.LIMELIGHT_SEEKING_RADIANS_PER_SECOND), - new AimAndFireRoutine(), - new MoveAwayFromAmp()); + /* new SeekTargetWithLimelight( + RobotContainer.limelight, + Constants.Limelight.AimingLimelight.PipelineNumbers.AMP_PIPELINE_NUMBER, + Constants.Limelight.LIMELIGHT_SEEKING_RADIANS_PER_SECOND),*/ + new AimAndFireRoutine(), new MoveAwayFromAmp()); } } From 8fd7a022cf301d40036fb78aed6cd569e33f66b2 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 08:38:44 -0500 Subject: [PATCH 10/53] Revert "limelight theoretically works I hope" This reverts commit 7357a64eff398ff5f58d42ba0b0d2f4b9dc1b7fd. --- .../frc/robot/commands/AimWithLimelight.java | 24 ++++--------------- 1 file changed, 5 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java index f7ab344..ebd14ec 100644 --- a/src/main/java/frc/robot/commands/AimWithLimelight.java +++ b/src/main/java/frc/robot/commands/AimWithLimelight.java @@ -12,21 +12,19 @@ package frc.robot.commands; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.subsystems.Drive; import frc.robot.subsystems.Limelight; -import swervelib.SwerveController; /** Aims with the limelight towards an object. */ public class AimWithLimelight extends Command { private Drive drive; private Limelight limelight; - private boolean finished, hasSeenTarget, isBlueAlliance; - private int counter, snapAngleXComponent; + private boolean finished, hasSeenTarget; + private int counter; + private double steerStrength, desiredDistanceFromTarget, mountHeight, @@ -144,17 +142,9 @@ private boolean isDistanced() { public void initialize() { this.oldPipelineNumber = limelight.getLimelightPipeline(); limelight.setLimelightPipeline(pipelineNumber); - drive.setHeadingCorrection(true); this.finished = false; this.hasSeenTarget = false; this.counter = 0; - - this.snapAngleXComponent = -1; - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false) { - this.snapAngleXComponent *= -1; - } } // Called every time the scheduler runs while the command is scheduled. @@ -165,13 +155,9 @@ public void execute() { /* Remember if we've seen a target so that we can assume we're aimed if we lose sight of the target * Only reason we assume this is because of where the Limelight is, which results in us not actually seeing the target if we're properly aimed */ - // hasSeenTarget = true; - - ChassisSpeeds desiredSpeeds = - drive.getTargetSpeeds(getX(), getRotation(), snapAngleXComponent, 0); + hasSeenTarget = true; - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - drive.driveRobot(translation, desiredSpeeds.omegaRadiansPerSecond, false); + drive.driveRobot(new Translation2d(getX() * -1.0, 0.0), -1 * getRotation(), false); /* End the command if we're at our "aimed" threshold */ if (isAngled() && isDistanced()) { From 08882674dafba8bfac9bd11ffa8b2a45957724b4 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 08:39:26 -0500 Subject: [PATCH 11/53] Revert "don't seek in auto" This reverts commit 0caf91d12bc82b750b0e165ce24e508d2cb48503. --- .../java/frc/robot/commands/auto/OnePieceAuto.java | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAuto.java b/src/main/java/frc/robot/commands/auto/OnePieceAuto.java index 01b653b..406c1b5 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAuto.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAuto.java @@ -12,7 +12,10 @@ package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.RobotContainer; import frc.robot.commands.AimAndFireRoutine; +import frc.robot.commands.SeekTargetWithLimelight; /* Enter weeb joke here. */ /** @@ -24,10 +27,11 @@ public class OnePieceAuto extends SequentialCommandGroup { /** Creates a new OnePieceAuto. */ public OnePieceAuto() { addCommands( - /* new SeekTargetWithLimelight( - RobotContainer.limelight, - Constants.Limelight.AimingLimelight.PipelineNumbers.AMP_PIPELINE_NUMBER, - Constants.Limelight.LIMELIGHT_SEEKING_RADIANS_PER_SECOND),*/ - new AimAndFireRoutine(), new MoveAwayFromAmp()); + new SeekTargetWithLimelight( + RobotContainer.limelight, + Constants.Limelight.AimingLimelight.PipelineNumbers.AMP_PIPELINE_NUMBER, + Constants.Limelight.LIMELIGHT_SEEKING_RADIANS_PER_SECOND), + new AimAndFireRoutine(), + new MoveAwayFromAmp()); } } From 53ca2db649f197502485bce33844138520285562 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 11:33:58 -0500 Subject: [PATCH 12/53] adjusted steer strength and lost a light Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4a5a541..5e85723 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -175,7 +175,7 @@ public static final class AimingLimelight { /** * How hard to turn toward the target. Double between 0 and 1, standard way to drive a motor */ - public static final double STEER_STRENGTH = 0.01; + public static final double STEER_STRENGTH = 0.02; /** How hard to drive toward the target. Same notation as above. */ public static final double DRIVE_STRENGTH = 0.01; @@ -233,7 +233,7 @@ public static class LightStrips { public static final int PWM_ID = 0; /** The count of LEDs for the lights. */ - public static final int LED_COUNT = 150; + public static final int LED_COUNT = 149; /** The speed of the fade animation. [0, 1] */ public static final double STRIP_FADE_SPEED = 0.05; From d5f3c4b09f57eaccb8bd6d3685b6cba6143d914f Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 13:09:18 -0500 Subject: [PATCH 13/53] shorter taxi Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5e85723..78d302c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -113,7 +113,7 @@ public static class Auto { public static final double TAXI_AUTO_METERS_PER_SECOND = 1.0; /** The number of seconds that we want to taxi for */ - public static final double TAXI_AUTO_DURATION_SECONDS = 2.0; + public static final double TAXI_AUTO_DURATION_SECONDS = 0.8; /** The amount of time we want/need to drive away from the amp in auto. */ public static final double DRIVE_AWAY_FROM_AMP_TIME = 2.0; From 6c3339b8e256f86a217f127bf79fb5145b1e8913 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:31:33 -0500 Subject: [PATCH 14/53] adjusted math Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- src/main/java/frc/robot/commands/AimWithLimelight.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java index ebd14ec..6a72a98 100644 --- a/src/main/java/frc/robot/commands/AimWithLimelight.java +++ b/src/main/java/frc/robot/commands/AimWithLimelight.java @@ -155,9 +155,9 @@ public void execute() { /* Remember if we've seen a target so that we can assume we're aimed if we lose sight of the target * Only reason we assume this is because of where the Limelight is, which results in us not actually seeing the target if we're properly aimed */ - hasSeenTarget = true; + // hasSeenTarget = true; - drive.driveRobot(new Translation2d(getX() * -1.0, 0.0), -1 * getRotation(), false); + drive.driveRobot(new Translation2d(getX(), getRotation()), 0, false); /* End the command if we're at our "aimed" threshold */ if (isAngled() && isDistanced()) { From e94ba230d4791b71df2234ee0fc3c7892a0c5d32 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:31:53 -0500 Subject: [PATCH 15/53] created red and blue drive to amp Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- .../robot/commands/auto/DriveToAmpBlue.java | 30 ++++++++++++++++ .../robot/commands/auto/DriveToAmpRed.java | 36 +++++++++++++++++++ 2 files changed, 66 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java create mode 100644 src/main/java/frc/robot/commands/auto/DriveToAmpRed.java diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java new file mode 100644 index 0000000..7674a62 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DriveToAmpBlue extends ParallelDeadlineGroup { + + /** Creates a new DriveToAmp. */ + public DriveToAmpBlue() { + super( + new WaitCommand(2), + new DriveFieldOrientedHeadingSnapping( + () -> 1.0, () -> 0.0, () -> 0.0, () -> false, () -> false, () -> true, () -> false)); + } +} diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java new file mode 100644 index 0000000..9150585 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DriveToAmpRed extends ParallelDeadlineGroup { + + /** Creates a new DriveToAmp. */ + public DriveToAmpRed() { + super( + new WaitCommand(2), + new DriveFieldOrientedHeadingSnapping( + () -> -1.0, + () -> 0.0, + () -> 0.0, + () -> false, + () -> false, + () -> false, + () -> true)); /* TODO: CONSTANTS */ + } +} From cae1030afea40f67cafa6f3b383240d28a3c100b Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:32:06 -0500 Subject: [PATCH 16/53] create dump note Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- .../frc/robot/commands/auto/DumpNote.java | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/DumpNote.java diff --git a/src/main/java/frc/robot/commands/auto/DumpNote.java b/src/main/java/frc/robot/commands/auto/DumpNote.java new file mode 100644 index 0000000..1ef7fcd --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DumpNote.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.mailbox.FireNoteRoutine; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DumpNote extends ParallelDeadlineGroup { + /** Creates a new DumpNote. */ + public DumpNote() { + // Add the deadline command in the super() call. Add other commands using + // addCommands(). + super(new WaitCommand(4), new FireNoteRoutine()); + // addCommands(new FooCommand(), new BarCommand()); + } +} From 4434e49d006d946d4aecba4eeb4a1dacb1e49a6d Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:32:31 -0500 Subject: [PATCH 17/53] create one porce auto but it works i swear Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- .../auto/OnePieceAutoButItWorksISwear.java | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java new file mode 100644 index 0000000..67b6531 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class OnePieceAutoButItWorksISwear extends SequentialCommandGroup { + /** Creates a new OnePieceAutoButItWorksISwear. */ + public OnePieceAutoButItWorksISwear() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false) { + addCommands(new DriveToAmpRed(), new DumpNote()); + } else { + addCommands(new DriveToAmpBlue(), new DumpNote()); + } + } +} From 9a01517b8110342c56260531c9d08ed8b2f33f01 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:32:44 -0500 Subject: [PATCH 18/53] create pick up from center auto Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- .../commands/auto/PickUpFromCenterAuto.java | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java diff --git a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java new file mode 100644 index 0000000..94252f9 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.RunIntake; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class PickUpFromCenterAuto extends ParallelCommandGroup { + /** Creates a new PickUpFromCenterAuto. */ + public PickUpFromCenterAuto() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new RunIntake(), + new ParallelDeadlineGroup( + new WaitCommand(5), new DriveAutoRobotOriented(new Translation2d(1, 0), 0))); + } +} From 8fe5760f8bd56b834dd47d72721138475f2de0af Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:32:55 -0500 Subject: [PATCH 19/53] create taxi long auto Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- .../frc/robot/commands/auto/TaxiLongAuto.java | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/TaxiLongAuto.java diff --git a/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java b/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java new file mode 100644 index 0000000..7c132bd --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java @@ -0,0 +1,31 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; + +/** + * Auto that just taxis (or more accurately, moves us out of the auto starting box.) Taxi time and + * velocity can be tuned in Constants.Drive. + */ +public class TaxiLongAuto extends ParallelDeadlineGroup { + /** Creates a new TaxiAuto. */ + public TaxiLongAuto() { + super(new WaitCommand(2)); + addCommands( + new DriveAutoRobotOriented( + new Translation2d(Constants.Auto.TAXI_AUTO_METERS_PER_SECOND, 0), 0)); + } +} From 1cd20cd4c440fed3b1d0d83833b5cf952a5dc49c Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:33:20 -0500 Subject: [PATCH 20/53] spotless Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- .../SetDrivePerspectiveFieldOrientedHeadingSnapping.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java index f5d79c5..2191e69 100644 --- a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java +++ b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java @@ -30,9 +30,7 @@ public SetDrivePerspectiveFieldOrientedHeadingSnapping() { @Override public void initialize() { drive.setDefaultCommand(RobotContainer.driveFieldOriented); - Controllers.pilotController - .leftBumper() - .whileTrue(RobotContainer.driveFieldOrientedHeadingSnapping); + Controllers.pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint); } // Called every time the scheduler runs while the command is scheduled. From 3c01f8018d0ce9cf0feacdc2cebe82fab6607576 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:33:35 -0500 Subject: [PATCH 21/53] adjusted taxi duration Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 78d302c..c79960c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -113,7 +113,7 @@ public static class Auto { public static final double TAXI_AUTO_METERS_PER_SECOND = 1.0; /** The number of seconds that we want to taxi for */ - public static final double TAXI_AUTO_DURATION_SECONDS = 0.8; + public static final double TAXI_AUTO_DURATION_SECONDS = 1.25; /** The amount of time we want/need to drive away from the amp in auto. */ public static final double DRIVE_AWAY_FROM_AMP_TIME = 2.0; From ccc33c3ab6f4e9fcaeff7b07d6bf2a268ebc333c Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:34:16 -0500 Subject: [PATCH 22/53] rebound left bumper to sprint on operatorless Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- src/main/java/frc/robot/Controllers.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index 76e7b90..889fc7f 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -103,7 +103,7 @@ private static void configureDefaultKeybinds() { operatorController.povDown().whileTrue(RobotContainer.runIntakeReverse); operatorController.leftTrigger().whileTrue(RobotContainer.aimToAmp); operatorController.rightTrigger().whileTrue(RobotContainer.fireNote); - pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedHeadingSnapping); + pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint); /* TODO: angle / velocity steering toggle w/ right trigger (no issue) */ @@ -123,6 +123,7 @@ private static void configureOperatorlessKeybinds() { pilotController.b().whileTrue(RobotContainer.fireNote); pilotController.x().whileTrue(RobotContainer.climbUp); pilotController.y().whileTrue(RobotContainer.climbDown); + pilotController.rightBumper().whileTrue(RobotContainer.driveFieldOrientedHeadingSnapping); keymapEntry.setString("Operatorless"); } From bd78e616f8eb0cab84a8664ed5fa37b07214b248 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:34:40 -0500 Subject: [PATCH 23/53] updated tobot container with autos Co-Authored-By: Gabe <91231142+willitcode@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16a3159..0703185 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,7 +31,10 @@ import frc.robot.commands.StartCamera; import frc.robot.commands.auto.MoveAwayFromAmp; import frc.robot.commands.auto.OnePieceAuto; +import frc.robot.commands.auto.OnePieceAutoButItWorksISwear; +import frc.robot.commands.auto.PickUpFromCenterAuto; import frc.robot.commands.auto.TaxiAuto; +import frc.robot.commands.auto.TaxiLongAuto; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; import frc.robot.commands.drive.DriveRobot; import frc.robot.commands.drive.SetDrivePerspectiveFieldOriented; @@ -261,6 +264,13 @@ public class RobotContainer { /** Singleton instance of {@link OnePieceAuto} for the whole robot. */ public static OnePieceAuto onePieceAuto = new OnePieceAuto(); + public static OnePieceAutoButItWorksISwear onePieceAutoButItWorksISwear = + new OnePieceAutoButItWorksISwear(); + + public static TaxiLongAuto taxiLongAuto = new TaxiLongAuto(); + + public static PickUpFromCenterAuto pickUpFromCenterAuto = new PickUpFromCenterAuto(); + /** Singleton instance of {@link TaxiAuto} for the whole robot. */ public static TaxiAuto taxiAuto = new TaxiAuto(); @@ -328,7 +338,13 @@ private void configureAuto() { /* This is where you put auto commands. Call autoChooser.addOption() to add autos. */ autoChooser.addOption("Taxi", taxiAuto); - autoChooser.addOption("One Note With Limelight", onePieceAuto); + // autoChooser.addOption("One Note With Limelight", onePieceAuto); + + autoChooser.addOption("WORKING ONE PIECE AUTO I HOPE", onePieceAutoButItWorksISwear); + + autoChooser.addOption("LONG taxi auto", taxiLongAuto); + + autoChooser.addOption("Pick Up Note From Center", pickUpFromCenterAuto); Shuffleboard.getTab("Driver").add("Choose Auto Routine", autoChooser); } From 02df367d0cdad7e33408e30bd925fa8a8a8d080a Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Sun, 7 Apr 2024 16:45:58 -0500 Subject: [PATCH 24/53] what --- .../robot/commands/auto/DriveToAmpBlue.java | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java index 7674a62..7995359 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -12,19 +12,37 @@ package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class DriveToAmpBlue extends ParallelDeadlineGroup { +public class DriveToAmpBlue extends SequentialCommandGroup { /** Creates a new DriveToAmp. */ public DriveToAmpBlue() { super( - new WaitCommand(2), - new DriveFieldOrientedHeadingSnapping( - () -> 1.0, () -> 0.0, () -> 0.0, () -> false, () -> false, () -> true, () -> false)); + new ParallelDeadlineGroup( + new WaitCommand(1), + new DriveFieldOrientedHeadingSnapping( + () -> 0.0, + () -> 0.0, + () -> 0.0, + () -> false, + () -> false, + () -> true, + () -> false)), + new ParallelDeadlineGroup( + new WaitCommand(0.5), + new DriveFieldOrientedHeadingSnapping( + () -> 0.0, + () -> 1.0, + () -> 0.0, + () -> false, + () -> false, + () -> false, + () -> false))); } } From 0641695f3de35f3a2ec841c590f2c6882aa2d96f Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Sun, 7 Apr 2024 16:46:31 -0500 Subject: [PATCH 25/53] what is a parallel deadline group --- .../robot/commands/auto/DriveToAmpRed.java | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java index 9150585..0d3f61d 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -12,25 +12,37 @@ package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class DriveToAmpRed extends ParallelDeadlineGroup { +public class DriveToAmpRed extends SequentialCommandGroup { /** Creates a new DriveToAmp. */ public DriveToAmpRed() { super( - new WaitCommand(2), - new DriveFieldOrientedHeadingSnapping( - () -> -1.0, - () -> 0.0, - () -> 0.0, - () -> false, - () -> false, - () -> false, - () -> true)); /* TODO: CONSTANTS */ + new ParallelDeadlineGroup( + new WaitCommand(1), + new DriveFieldOrientedHeadingSnapping( + () -> 0.0, + () -> 0.0, + () -> 0.0, + () -> false, + () -> false, + () -> false, + () -> true)), + new ParallelDeadlineGroup( + new WaitCommand(0.5), + new DriveFieldOrientedHeadingSnapping( + () -> 0.0, + () -> -1.0, + () -> 0.0, + () -> false, + () -> false, + () -> false, + () -> false))); /* TODO: CONSTANTS */ } } From 783b30585e98a0af91a846f442757783693c8c64 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Sun, 7 Apr 2024 16:46:44 -0500 Subject: [PATCH 26/53] create drive to center auto --- .../commands/auto/DriveToCenterAuto.java | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java diff --git a/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java new file mode 100644 index 0000000..9c00c3a --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DriveToCenterAuto extends ParallelCommandGroup { + /** Creates a new PickUpFromCenterAuto. */ + public DriveToCenterAuto() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new ParallelDeadlineGroup( + new WaitCommand(5), + new DriveFieldOrientedHeadingSnapping( + () -> 1.0, + () -> 0.0, + () -> 0.0, + () -> false, + () -> true, + () -> false, + () -> false))); + } +} From 3254130bcb5b938d84e141ae4246986b13c451e3 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Sun, 7 Apr 2024 16:57:22 -0500 Subject: [PATCH 27/53] bye bye translation --- .../robot/commands/auto/PickUpFromCenterAuto.java | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java index 94252f9..cfeed52 100644 --- a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java +++ b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java @@ -11,11 +11,11 @@ package frc.robot.commands.auto; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.RunIntake; +import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -28,6 +28,14 @@ public PickUpFromCenterAuto() { addCommands( new RunIntake(), new ParallelDeadlineGroup( - new WaitCommand(5), new DriveAutoRobotOriented(new Translation2d(1, 0), 0))); + new WaitCommand(5), + new DriveFieldOrientedHeadingSnapping( + () -> 1.0, + () -> 0.0, + () -> 0.0, + () -> false, + () -> true, + () -> false, + () -> false))); } } From 8da3a4f61312a96cb0b04a2bf9e4168bf3ce8f16 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Sun, 7 Apr 2024 16:57:59 -0500 Subject: [PATCH 28/53] set drive to stop in heading snapping end --- .../robot/commands/drive/DriveFieldOrientedHeadingSnapping.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java index b731dfe..3a98f3e 100644 --- a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java +++ b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java @@ -118,6 +118,7 @@ public void execute() { @Override public void end(boolean interrupted) { drive.setHeadingCorrection(false); + drive.stop(); } // Returns true when the command should end. From f7b902dbfe0bee2780d2df7922508a776add1a6c Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Sun, 7 Apr 2024 16:58:28 -0500 Subject: [PATCH 29/53] updated robotcontainer with drive to center auto --- src/main/java/frc/robot/RobotContainer.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0703185..4b8506e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.commands.RunIntake; import frc.robot.commands.RunIntakeReverse; import frc.robot.commands.StartCamera; +import frc.robot.commands.auto.DriveToCenterAuto; import frc.robot.commands.auto.MoveAwayFromAmp; import frc.robot.commands.auto.OnePieceAuto; import frc.robot.commands.auto.OnePieceAutoButItWorksISwear; @@ -271,6 +272,8 @@ public class RobotContainer { public static PickUpFromCenterAuto pickUpFromCenterAuto = new PickUpFromCenterAuto(); + public static DriveToCenterAuto driveToCenterAuto = new DriveToCenterAuto(); + /** Singleton instance of {@link TaxiAuto} for the whole robot. */ public static TaxiAuto taxiAuto = new TaxiAuto(); @@ -346,6 +349,8 @@ private void configureAuto() { autoChooser.addOption("Pick Up Note From Center", pickUpFromCenterAuto); + autoChooser.addOption("NO INTAKE drive to center", driveToCenterAuto); + Shuffleboard.getTab("Driver").add("Choose Auto Routine", autoChooser); } From a695784fe5738935574b5a9d4c3ede9f78d93753 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Tue, 9 Apr 2024 11:02:08 -0500 Subject: [PATCH 30/53] Revert "move belt follower call" This reverts commit 745ef5d45e393c9259005b0a87d88101623bf7f6. --- src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java b/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java index c75d03e..826bc09 100644 --- a/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java +++ b/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java @@ -32,12 +32,11 @@ public MailboxBelts() { lowerBeltMotor.setSmartCurrentLimit(Constants.MailboxBelts.BELT_MOTOR_CURRENT_LIMIT); upperBeltMotor.setInverted(Constants.MailboxBelts.UPPER_BELT_MOTOR_INVERTED); + lowerBeltMotor.follow(upperBeltMotor, Constants.MailboxBelts.BELTS_FOLLOWER_INVERSE_STATE); upperBeltMotor.setIdleMode(Constants.MailboxBelts.BELTS_IDLE_MODE); lowerBeltMotor.setIdleMode(Constants.MailboxBelts.BELTS_IDLE_MODE); - lowerBeltMotor.follow(upperBeltMotor, Constants.MailboxBelts.BELTS_FOLLOWER_INVERSE_STATE); - upperBeltMotor.burnFlash(); lowerBeltMotor.burnFlash(); } From 1cd5f0f3d666f0cd6b8d26c0bd6593d07a4a03a8 Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Mon, 22 Apr 2024 15:33:56 -0500 Subject: [PATCH 31/53] updated SDPFOHS to actually set the drive perspective to field oriented heading snapping --- .../SetDrivePerspectiveFieldOrientedHeadingSnapping.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java index 2191e69..f5d79c5 100644 --- a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java +++ b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java @@ -30,7 +30,9 @@ public SetDrivePerspectiveFieldOrientedHeadingSnapping() { @Override public void initialize() { drive.setDefaultCommand(RobotContainer.driveFieldOriented); - Controllers.pilotController.leftBumper().whileTrue(RobotContainer.driveFieldOrientedSprint); + Controllers.pilotController + .leftBumper() + .whileTrue(RobotContainer.driveFieldOrientedHeadingSnapping); } // Called every time the scheduler runs while the command is scheduled. From 791ff8be33b667351151ac278682e3b44e59d0cf Mon Sep 17 00:00:00 2001 From: Jack-Haefele <113944276+Jack-Haefele@users.noreply.github.com> Date: Mon, 22 Apr 2024 15:36:19 -0500 Subject: [PATCH 32/53] updated operatorless right bumper to drive field oriented sprint --- src/main/java/frc/robot/Controllers.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index 889fc7f..292a188 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -123,7 +123,7 @@ private static void configureOperatorlessKeybinds() { pilotController.b().whileTrue(RobotContainer.fireNote); pilotController.x().whileTrue(RobotContainer.climbUp); pilotController.y().whileTrue(RobotContainer.climbDown); - pilotController.rightBumper().whileTrue(RobotContainer.driveFieldOrientedHeadingSnapping); + pilotController.rightBumper().whileTrue(RobotContainer.driveFieldOrientedSprint); keymapEntry.setString("Operatorless"); } From c9ee874cf1cff5e6ff0ee06d1eaf4a3ab04502e3 Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Mon, 22 Apr 2024 16:13:20 -0500 Subject: [PATCH 33/53] add constants to DriveToAmpBlue --- src/main/java/frc/robot/Constants.java | 9 +++++++++ .../java/frc/robot/commands/auto/DriveToAmpBlue.java | 5 +++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c79960c..2c0f733 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -123,6 +123,15 @@ public static class Auto { /** The amount of time that we want to run the fire note command in auto. */ public static final double FIRE_NOTE_FOR_TIME = 4.0; + + /** Constants for the DriveToAmpBlue auto */ + public static class DriveToAmpBlue { + /** The time for the robot to drive left towards the amp. */ + public static final double LEFT_WAIT_TIME = 1; + + /** The time for the robot to drive forwards towards the amp. */ + public static final double FORWARD_WAIT_TIME = 0.1; + } } /** Constants for the Intake System */ diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java index 7995359..18619dc 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -25,7 +26,7 @@ public class DriveToAmpBlue extends SequentialCommandGroup { public DriveToAmpBlue() { super( new ParallelDeadlineGroup( - new WaitCommand(1), + new WaitCommand(Constants.Auto.DriveToAmpBlue.LEFT_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> 0.0, @@ -35,7 +36,7 @@ public DriveToAmpBlue() { () -> true, () -> false)), new ParallelDeadlineGroup( - new WaitCommand(0.5), + new WaitCommand(Constants.Auto.DriveToAmpBlue.FORWARD_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> 1.0, From dcebbd2ead2da2bb919968cfd76b92cc09b9d185 Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Mon, 22 Apr 2024 16:17:26 -0500 Subject: [PATCH 34/53] add constants to DriveToAmpRed --- src/main/java/frc/robot/Constants.java | 11 ++++++++++- .../java/frc/robot/commands/auto/DriveToAmpRed.java | 5 +++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2c0f733..dbe9d1d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -130,7 +130,16 @@ public static class DriveToAmpBlue { public static final double LEFT_WAIT_TIME = 1; /** The time for the robot to drive forwards towards the amp. */ - public static final double FORWARD_WAIT_TIME = 0.1; + public static final double FORWARD_WAIT_TIME = 0.5; + } + + /** Constants for the DriveToAmpRed auto */ + public static class DriveToAmpRed { + /** The time for the robot to drive left towards the amp. */ + public static final double LEFT_WAIT_TIME = 1; + + /** The time for the robot to drive forwards towards the amp. */ + public static final double FORWARD_WAIT_TIME = 0.5; } } diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java index 0d3f61d..6b6f756 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -25,7 +26,7 @@ public class DriveToAmpRed extends SequentialCommandGroup { public DriveToAmpRed() { super( new ParallelDeadlineGroup( - new WaitCommand(1), + new WaitCommand(Constants.Auto.DriveToAmpRed.LEFT_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> 0.0, @@ -35,7 +36,7 @@ public DriveToAmpRed() { () -> false, () -> true)), new ParallelDeadlineGroup( - new WaitCommand(0.5), + new WaitCommand(Constants.Auto.DriveToAmpRed.FORWARD_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> -1.0, From 9a9ed7e8aef0d0c8ee3c9f6041d4e9b9af2096c8 Mon Sep 17 00:00:00 2001 From: Berdenson <91093973+Berdenson@users.noreply.github.com> Date: Mon, 22 Apr 2024 16:23:48 -0500 Subject: [PATCH 35/53] AddressableLightStrip setColorLight javadoc --- src/main/java/frc/robot/subsystems/AddressableLightStrip.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/AddressableLightStrip.java b/src/main/java/frc/robot/subsystems/AddressableLightStrip.java index 0d1bdaa..e9a0571 100644 --- a/src/main/java/frc/robot/subsystems/AddressableLightStrip.java +++ b/src/main/java/frc/robot/subsystems/AddressableLightStrip.java @@ -46,8 +46,8 @@ public AddressableLightStrip(int pwmPort, int lightCount) { /** * Sets a light on the light strip to a {@link edu.wpi.first.wpilibj.util.Color} * - * @param lightNumber - * @param color + * @param lightNumber the number of the LED. + * @param color The color to set the LED to. */ public void setColorLight(int lightNumber, Color color) { if (lightNumber > this.buffer.getLength()) { From 6e89bf0c382e0e1e7fe4a7700373726bfde76a1c Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Mon, 22 Apr 2024 16:25:47 -0500 Subject: [PATCH 36/53] add DriveToCenter wait time constant --- src/main/java/frc/robot/Constants.java | 4 ++++ src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java | 3 ++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dbe9d1d..1063cf1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -141,6 +141,10 @@ public static class DriveToAmpRed { /** The time for the robot to drive forwards towards the amp. */ public static final double FORWARD_WAIT_TIME = 0.5; } + + public static class DriveToCenter { + public static final double DRIVE_CENTER_WAIT_TIME = 5.0; + } } /** Constants for the Intake System */ diff --git a/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java index 9c00c3a..db40369 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java +++ b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -26,7 +27,7 @@ public DriveToCenterAuto() { // addCommands(new FooCommand(), new BarCommand()); addCommands( new ParallelDeadlineGroup( - new WaitCommand(5), + new WaitCommand(Constants.Auto.DriveToCenter.DRIVE_CENTER_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 1.0, () -> 0.0, From ac08d99062f34d20da542eaaf704ca64d824cb0a Mon Sep 17 00:00:00 2001 From: Berdenson <91093973+Berdenson@users.noreply.github.com> Date: Mon, 22 Apr 2024 16:25:49 -0500 Subject: [PATCH 37/53] DriveToAmps javadocs --- src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java | 3 ++- src/main/java/frc/robot/commands/auto/DriveToAmpRed.java | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java index 7995359..ab78a64 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -19,9 +19,10 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +/** Drives bot to blue amp. */ public class DriveToAmpBlue extends SequentialCommandGroup { - /** Creates a new DriveToAmp. */ + /** Creates a new DriveToAmpBlue. */ public DriveToAmpBlue() { super( new ParallelDeadlineGroup( diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java index 0d3f61d..d05a3d1 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -19,9 +19,10 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +/** Drives bot to red amp. */ public class DriveToAmpRed extends SequentialCommandGroup { - /** Creates a new DriveToAmp. */ + /** Creates a new DriveToAmpRed. */ public DriveToAmpRed() { super( new ParallelDeadlineGroup( From a4d09b8852d08074112a8efe3d3b667a90cfede1 Mon Sep 17 00:00:00 2001 From: Berdenson <91093973+Berdenson@users.noreply.github.com> Date: Mon, 22 Apr 2024 16:27:03 -0500 Subject: [PATCH 38/53] javadoc DriveToCenterAuto --- src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java | 2 +- src/main/java/frc/robot/commands/auto/DriveToAmpRed.java | 2 +- src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java index ab78a64..2caab63 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -19,7 +19,7 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -/** Drives bot to blue amp. */ +/** Auto that drives bot to blue amp. */ public class DriveToAmpBlue extends SequentialCommandGroup { /** Creates a new DriveToAmpBlue. */ diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java index d05a3d1..91f7cb8 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -19,7 +19,7 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -/** Drives bot to red amp. */ +/** Auto that drives bot to red amp. */ public class DriveToAmpRed extends SequentialCommandGroup { /** Creates a new DriveToAmpRed. */ diff --git a/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java index 9c00c3a..d6aac6c 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java +++ b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java @@ -19,6 +19,7 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +/** Auto that drives bot to center of field. */ public class DriveToCenterAuto extends ParallelCommandGroup { /** Creates a new PickUpFromCenterAuto. */ public DriveToCenterAuto() { From 02f53ab21629ccc5a96da2fadb1fdeecec8fda2a Mon Sep 17 00:00:00 2001 From: Berdenson <91093973+Berdenson@users.noreply.github.com> Date: Mon, 22 Apr 2024 16:28:12 -0500 Subject: [PATCH 39/53] DumpNote javadoc --- src/main/java/frc/robot/commands/auto/DumpNote.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/commands/auto/DumpNote.java b/src/main/java/frc/robot/commands/auto/DumpNote.java index 1ef7fcd..1475b47 100644 --- a/src/main/java/frc/robot/commands/auto/DumpNote.java +++ b/src/main/java/frc/robot/commands/auto/DumpNote.java @@ -18,6 +18,7 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +/** Auto that dumps the preloaded note in the robot after 4 seconds. */ public class DumpNote extends ParallelDeadlineGroup { /** Creates a new DumpNote. */ public DumpNote() { From cf0324e095cf7e879aab7d66699087a4ef0db989 Mon Sep 17 00:00:00 2001 From: Berdenson <91093973+Berdenson@users.noreply.github.com> Date: Mon, 22 Apr 2024 16:29:34 -0500 Subject: [PATCH 40/53] OnePieceAutoButItWorksISwear javadoc --- .../frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java index 67b6531..9bb17c5 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java @@ -17,6 +17,8 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +// One piece - the anime thing +/** Auto that drives to the amp and offloads the note. */ public class OnePieceAutoButItWorksISwear extends SequentialCommandGroup { /** Creates a new OnePieceAutoButItWorksISwear. */ public OnePieceAutoButItWorksISwear() { From 8afa2fd853b8301d5b8e1221d49c0e9fe739446c Mon Sep 17 00:00:00 2001 From: Berdenson <91093973+Berdenson@users.noreply.github.com> Date: Mon, 22 Apr 2024 16:30:39 -0500 Subject: [PATCH 41/53] PickUpFromCenterAuto javadoc --- src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java index cfeed52..613871c 100644 --- a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java +++ b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java @@ -20,6 +20,7 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +/** Auto that drives the bot to a note and intakes it. */ public class PickUpFromCenterAuto extends ParallelCommandGroup { /** Creates a new PickUpFromCenterAuto. */ public PickUpFromCenterAuto() { From 9c6cdd54e9fdf09df8ecbf2f66db92a80a4a2534 Mon Sep 17 00:00:00 2001 From: Berdenson <91093973+Berdenson@users.noreply.github.com> Date: Mon, 22 Apr 2024 16:32:54 -0500 Subject: [PATCH 42/53] added javadoc to public instances in robotContainer --- src/main/java/frc/robot/RobotContainer.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4b8506e..9621e7c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -265,13 +265,17 @@ public class RobotContainer { /** Singleton instance of {@link OnePieceAuto} for the whole robot. */ public static OnePieceAuto onePieceAuto = new OnePieceAuto(); + /** Singleton instance of {@link OnePieceAutoButItWorksISwear} for the whole robot. */ public static OnePieceAutoButItWorksISwear onePieceAutoButItWorksISwear = new OnePieceAutoButItWorksISwear(); + /** Singleton instance of {@link TaxiLongAuto} for the whole robot. */ public static TaxiLongAuto taxiLongAuto = new TaxiLongAuto(); + /** Singleton instance of {@link PickUpFromCenterAuto} for the whole robot. */ public static PickUpFromCenterAuto pickUpFromCenterAuto = new PickUpFromCenterAuto(); + /** Singleton instance of {@link DriveToCenterAuto} for the whole robot. */ public static DriveToCenterAuto driveToCenterAuto = new DriveToCenterAuto(); /** Singleton instance of {@link TaxiAuto} for the whole robot. */ @@ -286,7 +290,7 @@ public class RobotContainer { /** Singleton instance of {@link DisabledLight} for the whole robot. */ public static DisabledLight disabledLights = new DisabledLight(); - /** Singleton instance of {@link EnableLights} for the whole robot. */ + /** Singleton instance of {@link EnabledLight} for the whole robot. */ public static EnabledLight enabledLights = new EnabledLight(); /** Singleton instance of {@link NoteLight} for the whole robot. */ From 8ea04d6d38a9d74dadc7361997e096484ad8d8a7 Mon Sep 17 00:00:00 2001 From: Berdenson <91093973+Berdenson@users.noreply.github.com> Date: Mon, 22 Apr 2024 16:51:26 -0500 Subject: [PATCH 43/53] javadoc autos --- src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java | 2 +- src/main/java/frc/robot/commands/auto/DriveToAmpRed.java | 2 +- .../frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java index 2caab63..49b4348 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -19,7 +19,7 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -/** Auto that drives bot to blue amp. */ +/** Auto that manually drives bot to blue amp. Made due to faulty limelight at comp. */ public class DriveToAmpBlue extends SequentialCommandGroup { /** Creates a new DriveToAmpBlue. */ diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java index 91f7cb8..75f9a6a 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -19,7 +19,7 @@ // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -/** Auto that drives bot to red amp. */ +/** Auto that manually drives bot to red amp. Made due to faulty limelight at comp. */ public class DriveToAmpRed extends SequentialCommandGroup { /** Creates a new DriveToAmpRed. */ diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java index 9bb17c5..073a801 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java @@ -18,7 +18,7 @@ // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html // One piece - the anime thing -/** Auto that drives to the amp and offloads the note. */ +/** Auto that manually drives to the amp and offloads the note. Made due to faulty limelight at comp. */ public class OnePieceAutoButItWorksISwear extends SequentialCommandGroup { /** Creates a new OnePieceAutoButItWorksISwear. */ public OnePieceAutoButItWorksISwear() { From 663e4e16319f2071e804ff636c8b6f5e203e03d3 Mon Sep 17 00:00:00 2001 From: Berdenson <91093973+Berdenson@users.noreply.github.com> Date: Mon, 29 Apr 2024 15:20:40 -0500 Subject: [PATCH 44/53] spotless --- .../frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java index 073a801..0367ff2 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java @@ -18,7 +18,9 @@ // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html // One piece - the anime thing -/** Auto that manually drives to the amp and offloads the note. Made due to faulty limelight at comp. */ +/** + * Auto that manually drives to the amp and offloads the note. Made due to faulty limelight at comp. + */ public class OnePieceAutoButItWorksISwear extends SequentialCommandGroup { /** Creates a new OnePieceAutoButItWorksISwear. */ public OnePieceAutoButItWorksISwear() { From b4e9711c33c245da363e9a7d7c68348a577d896a Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Mon, 29 Apr 2024 16:36:45 -0500 Subject: [PATCH 45/53] Add getAlliance connivence method --- src/main/java/frc/robot/RobotContainer.java | 18 ++++++++++++++++++ .../commands/SeekTargetWithLimelight.java | 3 +-- .../auto/OnePieceAutoButItWorksISwear.java | 6 +++--- .../commands/lightstrip/EnabledLight.java | 15 +++++---------- src/main/java/frc/robot/subsystems/Drive.java | 7 ++----- 5 files changed, 29 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4b8506e..f036335 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; @@ -63,6 +65,8 @@ import frc.robot.subsystems.mailbox.Mailbox; import frc.robot.subsystems.mailbox.MailboxBelts; import frc.robot.subsystems.mailbox.MailboxPneumatics; +import java.util.Optional; +import org.jspecify.annotations.Nullable; /** Singleton class that contains all the robot's subsystems, commands, and button bindings. */ @SuppressWarnings("unused") @@ -377,4 +381,18 @@ private void configureBindings() { public Command getAutonomousCommand() { return autoChooser.getSelected(); } + + /** + * Gets the current alliance. Returns null if there is no alliance. + * + * @return the current alliance. Nullable. + */ + public static @Nullable Alliance getAlliance() { + Optional alliance = DriverStation.getAlliance(); + if (!alliance.isEmpty()) { + return alliance.get(); + } else { + return null; + } + } } diff --git a/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java b/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java index 81e4154..099cb69 100644 --- a/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java +++ b/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java @@ -52,8 +52,7 @@ public void initialize() { this.oldPipelineNumber = limelight.getLimelightPipeline(); limelight.setLimelightPipeline(seekingPipelineNumber); - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false) { + if (RobotContainer.getAlliance() == DriverStation.Alliance.Red) { this.rotationRadiansPerSecond = rotationRadiansPerSecond * -1; } } diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java index 67b6531..cce20d4 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java @@ -11,8 +11,9 @@ package frc.robot.commands.auto; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotContainer; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -22,8 +23,7 @@ public class OnePieceAutoButItWorksISwear extends SequentialCommandGroup { public OnePieceAutoButItWorksISwear() { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false) { + if (RobotContainer.getAlliance() == Alliance.Red) { addCommands(new DriveToAmpRed(), new DumpNote()); } else { addCommands(new DriveToAmpBlue(), new DumpNote()); diff --git a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java index 5f3bbe7..ee228dd 100644 --- a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java +++ b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java @@ -11,13 +11,11 @@ package frc.robot.commands.lightstrip; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.subsystems.AddressableLightStrip; -import java.util.Optional; /** Activates the light strip when the robot is enabled. */ public class EnabledLight extends Command { @@ -32,16 +30,13 @@ public EnabledLight() { // Called when the command is initially scheduled. @Override public void initialize() { - Optional alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - if (alliance.get() == Alliance.Red) { - robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_RED_ALLIANCE); - } else { - robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_BLUE_ALLIANCE); - } + + if (RobotContainer.getAlliance() == Alliance.Red) { + robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_RED_ALLIANCE); } else { - robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_NO_ALLIANCE); + robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_BLUE_ALLIANCE); } + robotLights.flush(); robotLights.startLights(); } diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index fea140c..3228d29 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.RobotContainer; import java.io.File; import java.io.IOException; import swervelib.SwerveDrive; @@ -79,11 +80,7 @@ public Drive() { /* This will flip the path being followed to the red side of the field. */ /* THE ORIGIN WILL REMAIN ON THE BLUE SIDE */ - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; + return RobotContainer.getAlliance() == DriverStation.Alliance.Red; }, this /* Reference to this subsystem to set requirements */); } From fc6fb12c5f3a59d8a1ed2c108de31b0b6097db5b Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Mon, 29 Apr 2024 16:41:59 -0500 Subject: [PATCH 46/53] remove WPIlib comments --- src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java | 3 --- src/main/java/frc/robot/commands/auto/DriveToAmpRed.java | 5 +---- .../java/frc/robot/commands/auto/DriveToCenterAuto.java | 5 ----- src/main/java/frc/robot/commands/auto/DumpNote.java | 6 ------ .../robot/commands/auto/OnePieceAutoButItWorksISwear.java | 6 +----- .../java/frc/robot/commands/auto/PickUpFromCenterAuto.java | 6 +----- 6 files changed, 3 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java index 18619dc..9ac44e8 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -17,9 +17,6 @@ import frc.robot.Constants; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class DriveToAmpBlue extends SequentialCommandGroup { /** Creates a new DriveToAmp. */ diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java index 6b6f756..6e201f8 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -17,9 +17,6 @@ import frc.robot.Constants; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class DriveToAmpRed extends SequentialCommandGroup { /** Creates a new DriveToAmp. */ @@ -44,6 +41,6 @@ public DriveToAmpRed() { () -> false, () -> false, () -> false, - () -> false))); /* TODO: CONSTANTS */ + () -> false))); } } diff --git a/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java index db40369..740aa61 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java +++ b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.java @@ -17,14 +17,9 @@ import frc.robot.Constants; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class DriveToCenterAuto extends ParallelCommandGroup { /** Creates a new PickUpFromCenterAuto. */ public DriveToCenterAuto() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); addCommands( new ParallelDeadlineGroup( new WaitCommand(Constants.Auto.DriveToCenter.DRIVE_CENTER_WAIT_TIME), diff --git a/src/main/java/frc/robot/commands/auto/DumpNote.java b/src/main/java/frc/robot/commands/auto/DumpNote.java index 1ef7fcd..796781c 100644 --- a/src/main/java/frc/robot/commands/auto/DumpNote.java +++ b/src/main/java/frc/robot/commands/auto/DumpNote.java @@ -15,15 +15,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.mailbox.FireNoteRoutine; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class DumpNote extends ParallelDeadlineGroup { /** Creates a new DumpNote. */ public DumpNote() { - // Add the deadline command in the super() call. Add other commands using - // addCommands(). super(new WaitCommand(4), new FireNoteRoutine()); - // addCommands(new FooCommand(), new BarCommand()); } } diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java index cce20d4..07c0c33 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java @@ -15,14 +15,10 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.RobotContainer; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class OnePieceAutoButItWorksISwear extends SequentialCommandGroup { /** Creates a new OnePieceAutoButItWorksISwear. */ public OnePieceAutoButItWorksISwear() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); + if (RobotContainer.getAlliance() == Alliance.Red) { addCommands(new DriveToAmpRed(), new DumpNote()); } else { diff --git a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java index cfeed52..0202370 100644 --- a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java +++ b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java @@ -17,14 +17,10 @@ import frc.robot.commands.RunIntake; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class PickUpFromCenterAuto extends ParallelCommandGroup { /** Creates a new PickUpFromCenterAuto. */ public PickUpFromCenterAuto() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); + addCommands( new RunIntake(), new ParallelDeadlineGroup( From b82308fa18f72859b3da6576ecd46081aa49062e Mon Sep 17 00:00:00 2001 From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com> Date: Mon, 29 Apr 2024 16:42:16 -0500 Subject: [PATCH 47/53] Basic skeleton --- src/main/java/frc/robot/Constants.java | 3 +++ .../robot/commands/auto/DelayedTaxiAuto.java | 26 +++++++++++++++++++ 2 files changed, 29 insertions(+) create mode 100644 src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c79960c..33a9060 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -123,6 +123,9 @@ public static class Auto { /** The amount of time that we want to run the fire note command in auto. */ public static final double FIRE_NOTE_FOR_TIME = 4.0; + + /** Time in seconds that we delay our taxi in the delayed taxi auto. */ + public static final double TAXI_DELAY_TIME = 10.0; } /** Constants for the Intake System */ diff --git a/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java b/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java new file mode 100644 index 0000000..9a68507 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Asimov's Laws: + * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm. + * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law. + * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law. + */ + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; + +/** Taxi auto that waits a bit before it taxis. */ +public class DelayedTaxiAuto extends SequentialCommandGroup { + /** Creates a new DelayedTaxiAuto. */ + public DelayedTaxiAuto() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new WaitCommand(Constants.Auto.TAXI_DELAY_TIME), new TaxiAuto()); + } +} From 96049b0e23a7c06c3e53e0c556e09739bb6a6d30 Mon Sep 17 00:00:00 2001 From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com> Date: Mon, 29 Apr 2024 16:46:58 -0500 Subject: [PATCH 48/53] Added to RobotContainer --- src/main/java/frc/robot/RobotContainer.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4b8506e..3aca87f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.commands.RunIntake; import frc.robot.commands.RunIntakeReverse; import frc.robot.commands.StartCamera; +import frc.robot.commands.auto.DelayedTaxiAuto; import frc.robot.commands.auto.DriveToCenterAuto; import frc.robot.commands.auto.MoveAwayFromAmp; import frc.robot.commands.auto.OnePieceAuto; @@ -277,6 +278,9 @@ public class RobotContainer { /** Singleton instance of {@link TaxiAuto} for the whole robot. */ public static TaxiAuto taxiAuto = new TaxiAuto(); + /** Singleton instance of {@link DelayedTaxiAuto} for the whole robot. */ + public static DelayedTaxiAuto delayedTaxiAuto = new DelayedTaxiAuto(); + /** Singleton instance of {@link ClearPDPStickyFaults} for the whole robot. */ public static ClearPDPStickyFaults clearPDPStickyFaults = new ClearPDPStickyFaults(); @@ -351,6 +355,8 @@ private void configureAuto() { autoChooser.addOption("NO INTAKE drive to center", driveToCenterAuto); + autoChooser.addOption("Taxi with 10 second delay", delayedTaxiAuto); + Shuffleboard.getTab("Driver").add("Choose Auto Routine", autoChooser); } From 83057e1bdecbf21df08b00cd01fbaf9a56bef781 Mon Sep 17 00:00:00 2001 From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com> Date: Mon, 29 Apr 2024 16:59:19 -0500 Subject: [PATCH 49/53] Removed comments --- src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java b/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java index 9a68507..2676f41 100644 --- a/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java +++ b/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java @@ -19,8 +19,6 @@ public class DelayedTaxiAuto extends SequentialCommandGroup { /** Creates a new DelayedTaxiAuto. */ public DelayedTaxiAuto() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); addCommands(new WaitCommand(Constants.Auto.TAXI_DELAY_TIME), new TaxiAuto()); } } From e7bf22af707488c843286026340f35d5f85da9c2 Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Mon, 6 May 2024 16:34:37 -0500 Subject: [PATCH 50/53] fix more stuff --- src/main/java/frc/robot/Constants.java | 6 ++++++ src/main/java/frc/robot/RobotContainer.java | 7 +++---- .../java/frc/robot/commands/SeekTargetWithLimelight.java | 3 +-- src/main/java/frc/robot/commands/auto/DumpNote.java | 3 ++- .../robot/commands/auto/OnePieceAutoButItWorksISwear.java | 3 +-- .../java/frc/robot/commands/auto/PickUpFromCenterAuto.java | 3 ++- .../java/frc/robot/commands/lightstrip/EnabledLight.java | 3 +-- src/main/java/frc/robot/subsystems/Drive.java | 3 +-- 8 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 68cfa6d..4bf1b16 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -127,6 +127,12 @@ public static class Auto { /** Time in seconds that we delay our taxi in the delayed taxi auto. */ public static final double TAXI_DELAY_TIME = 10.0; + /** Time in seconds that we wait for FireNoteRoutine to end. */ + public static final double DUMP_NOTE_WAIT_TIME = 4.0; + + /** Time in seconds that we wait for PickupFromCenter to end. */ + public static final double PICKUP_CENTER_WAIT_TIME = 5.0; + /** Constants for the DriveToAmpBlue auto */ public static class DriveToAmpBlue { /** The time for the robot to drive left towards the amp. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4232d5f..f355374 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,7 +67,6 @@ import frc.robot.subsystems.mailbox.MailboxBelts; import frc.robot.subsystems.mailbox.MailboxPneumatics; import java.util.Optional; -import org.jspecify.annotations.Nullable; /** Singleton class that contains all the robot's subsystems, commands, and button bindings. */ @SuppressWarnings("unused") @@ -397,12 +396,12 @@ public Command getAutonomousCommand() { * * @return the current alliance. Nullable. */ - public static @Nullable Alliance getAlliance() { + public static boolean getAlliance() { Optional alliance = DriverStation.getAlliance(); if (!alliance.isEmpty()) { - return alliance.get(); + return alliance.get() == Alliance.Red; } else { - return null; + return false; } } } diff --git a/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java b/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java index 099cb69..20857ea 100644 --- a/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java +++ b/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java @@ -11,7 +11,6 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.RobotContainer; @@ -52,7 +51,7 @@ public void initialize() { this.oldPipelineNumber = limelight.getLimelightPipeline(); limelight.setLimelightPipeline(seekingPipelineNumber); - if (RobotContainer.getAlliance() == DriverStation.Alliance.Red) { + if (RobotContainer.getAlliance()) { this.rotationRadiansPerSecond = rotationRadiansPerSecond * -1; } } diff --git a/src/main/java/frc/robot/commands/auto/DumpNote.java b/src/main/java/frc/robot/commands/auto/DumpNote.java index bc2255e..8977202 100644 --- a/src/main/java/frc/robot/commands/auto/DumpNote.java +++ b/src/main/java/frc/robot/commands/auto/DumpNote.java @@ -13,12 +13,13 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.mailbox.FireNoteRoutine; /** Auto that dumps the preloaded note in the robot after 4 seconds. */ public class DumpNote extends ParallelDeadlineGroup { /** Creates a new DumpNote. */ public DumpNote() { - super(new WaitCommand(4), new FireNoteRoutine()); + super(new WaitCommand(Constants.Auto.DUMP_NOTE_WAIT_TIME), new FireNoteRoutine()); } } diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java index e468d8e..a5f3216 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java @@ -11,7 +11,6 @@ package frc.robot.commands.auto; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.RobotContainer; @@ -23,7 +22,7 @@ public class OnePieceAutoButItWorksISwear extends SequentialCommandGroup { /** Creates a new OnePieceAutoButItWorksISwear. */ public OnePieceAutoButItWorksISwear() { - if (RobotContainer.getAlliance() == Alliance.Red) { + if (RobotContainer.getAlliance()) { addCommands(new DriveToAmpRed(), new DumpNote()); } else { addCommands(new DriveToAmpBlue(), new DumpNote()); diff --git a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java index 446b64c..ed09b49 100644 --- a/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java +++ b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.RunIntake; import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; @@ -25,7 +26,7 @@ public PickUpFromCenterAuto() { addCommands( new RunIntake(), new ParallelDeadlineGroup( - new WaitCommand(5), + new WaitCommand(Constants.Auto.PICKUP_CENTER_WAIT_TIME), new DriveFieldOrientedHeadingSnapping( () -> 1.0, () -> 0.0, diff --git a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java index ee228dd..4b58b6b 100644 --- a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java +++ b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java @@ -11,7 +11,6 @@ package frc.robot.commands.lightstrip; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.RobotContainer; @@ -31,7 +30,7 @@ public EnabledLight() { @Override public void initialize() { - if (RobotContainer.getAlliance() == Alliance.Red) { + if (RobotContainer.getAlliance()) { robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_RED_ALLIANCE); } else { robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_BLUE_ALLIANCE); diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 3228d29..24194eb 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -19,7 +19,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -80,7 +79,7 @@ public Drive() { /* This will flip the path being followed to the red side of the field. */ /* THE ORIGIN WILL REMAIN ON THE BLUE SIDE */ - return RobotContainer.getAlliance() == DriverStation.Alliance.Red; + return RobotContainer.getAlliance(); }, this /* Reference to this subsystem to set requirements */); } From 220173a989c2bdc1a30098575e945545edaf06a0 Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Mon, 13 May 2024 16:05:06 -0500 Subject: [PATCH 51/53] more tiny fixes --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../java/frc/robot/commands/SeekTargetWithLimelight.java | 2 +- .../robot/commands/auto/OnePieceAutoButItWorksISwear.java | 2 +- .../java/frc/robot/commands/lightstrip/EnabledLight.java | 2 +- src/main/java/frc/robot/subsystems/Drive.java | 2 +- 6 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4bf1b16..9263586 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -151,7 +151,9 @@ public static class DriveToAmpRed { public static final double FORWARD_WAIT_TIME = 0.5; } + /** Constants for the auto that drives bot to center of field. */ public static class DriveToCenter { + /** The time for the robot to drive to the center. */ public static final double DRIVE_CENTER_WAIT_TIME = 5.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f355374..3efc3e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -392,11 +392,11 @@ public Command getAutonomousCommand() { } /** - * Gets the current alliance. Returns null if there is no alliance. + * Returns true if the robot is on the red alliance. False otherwise. * - * @return the current alliance. Nullable. + * @return True if the robot is on the red alliance. False otherwise. */ - public static boolean getAlliance() { + public static boolean isRedAlliance() { Optional alliance = DriverStation.getAlliance(); if (!alliance.isEmpty()) { return alliance.get() == Alliance.Red; diff --git a/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java b/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java index 20857ea..923d150 100644 --- a/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java +++ b/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java @@ -51,7 +51,7 @@ public void initialize() { this.oldPipelineNumber = limelight.getLimelightPipeline(); limelight.setLimelightPipeline(seekingPipelineNumber); - if (RobotContainer.getAlliance()) { + if (RobotContainer.isRedAlliance()) { this.rotationRadiansPerSecond = rotationRadiansPerSecond * -1; } } diff --git a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java index a5f3216..35bae2e 100644 --- a/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.java @@ -22,7 +22,7 @@ public class OnePieceAutoButItWorksISwear extends SequentialCommandGroup { /** Creates a new OnePieceAutoButItWorksISwear. */ public OnePieceAutoButItWorksISwear() { - if (RobotContainer.getAlliance()) { + if (RobotContainer.isRedAlliance()) { addCommands(new DriveToAmpRed(), new DumpNote()); } else { addCommands(new DriveToAmpBlue(), new DumpNote()); diff --git a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java index 4b58b6b..bc1af95 100644 --- a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java +++ b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java @@ -30,7 +30,7 @@ public EnabledLight() { @Override public void initialize() { - if (RobotContainer.getAlliance()) { + if (RobotContainer.isRedAlliance()) { robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_RED_ALLIANCE); } else { robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_BLUE_ALLIANCE); diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 24194eb..1c21d49 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -79,7 +79,7 @@ public Drive() { /* This will flip the path being followed to the red side of the field. */ /* THE ORIGIN WILL REMAIN ON THE BLUE SIDE */ - return RobotContainer.getAlliance(); + return RobotContainer.isRedAlliance(); }, this /* Reference to this subsystem to set requirements */); } From 17264d4cca990d3e47f0d3a1a571540a4a93483c Mon Sep 17 00:00:00 2001 From: QuackitsQuinn Date: Mon, 13 May 2024 16:25:12 -0500 Subject: [PATCH 52/53] tiny constant --- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/commands/auto/TaxiLongAuto.java | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9263586..af380a3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -133,6 +133,9 @@ public static class Auto { /** Time in seconds that we wait for PickupFromCenter to end. */ public static final double PICKUP_CENTER_WAIT_TIME = 5.0; + /** Time in seconds that we wait for TaxiLongAuto to end. */ + public static final double TAXI_LONG_WAIT_TIME = 2.0; + /** Constants for the DriveToAmpBlue auto */ public static class DriveToAmpBlue { /** The time for the robot to drive left towards the amp. */ diff --git a/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java b/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java index 7c132bd..8822079 100644 --- a/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java +++ b/src/main/java/frc/robot/commands/auto/TaxiLongAuto.java @@ -23,7 +23,7 @@ public class TaxiLongAuto extends ParallelDeadlineGroup { /** Creates a new TaxiAuto. */ public TaxiLongAuto() { - super(new WaitCommand(2)); + super(new WaitCommand(Constants.Auto.TAXI_LONG_WAIT_TIME)); addCommands( new DriveAutoRobotOriented( new Translation2d(Constants.Auto.TAXI_AUTO_METERS_PER_SECOND, 0), 0)); From bfcf1be9b6649297143fbd0c44000fc98a21951e Mon Sep 17 00:00:00 2001 From: willitcode <91231142+willitcode@users.noreply.github.com> Date: Mon, 13 May 2024 17:15:19 -0500 Subject: [PATCH 53/53] fix duplicated constants --- src/main/java/frc/robot/Constants.java | 21 ++++++------------- .../robot/commands/auto/DriveToAmpBlue.java | 4 ++-- .../robot/commands/auto/DriveToAmpRed.java | 4 ++-- 3 files changed, 10 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index af380a3..0f496d5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -136,22 +136,13 @@ public static class Auto { /** Time in seconds that we wait for TaxiLongAuto to end. */ public static final double TAXI_LONG_WAIT_TIME = 2.0; - /** Constants for the DriveToAmpBlue auto */ - public static class DriveToAmpBlue { - /** The time for the robot to drive left towards the amp. */ - public static final double LEFT_WAIT_TIME = 1; + /** Constants for the DriveToAmp autos */ + public static class DriveToAmp { + /** The time that the robot spends snapping to the appropriate angle. */ + public static final double SNAPPING_TIME = 1; - /** The time for the robot to drive forwards towards the amp. */ - public static final double FORWARD_WAIT_TIME = 0.5; - } - - /** Constants for the DriveToAmpRed auto */ - public static class DriveToAmpRed { - /** The time for the robot to drive left towards the amp. */ - public static final double LEFT_WAIT_TIME = 1; - - /** The time for the robot to drive forwards towards the amp. */ - public static final double FORWARD_WAIT_TIME = 0.5; + /** The time for the robot to drive towards the amp. */ + public static final double DRIVE_TO_AMP_TIME = 0.5; } /** Constants for the auto that drives bot to center of field. */ diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java index 30c4b38..22444a3 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -24,7 +24,7 @@ public class DriveToAmpBlue extends SequentialCommandGroup { public DriveToAmpBlue() { super( new ParallelDeadlineGroup( - new WaitCommand(Constants.Auto.DriveToAmpBlue.LEFT_WAIT_TIME), + new WaitCommand(Constants.Auto.DriveToAmp.SNAPPING_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> 0.0, @@ -34,7 +34,7 @@ public DriveToAmpBlue() { () -> true, () -> false)), new ParallelDeadlineGroup( - new WaitCommand(Constants.Auto.DriveToAmpBlue.FORWARD_WAIT_TIME), + new WaitCommand(Constants.Auto.DriveToAmp.DRIVE_TO_AMP_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> 1.0, diff --git a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java index 8b4cc19..a542120 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -24,7 +24,7 @@ public class DriveToAmpRed extends SequentialCommandGroup { public DriveToAmpRed() { super( new ParallelDeadlineGroup( - new WaitCommand(Constants.Auto.DriveToAmpRed.LEFT_WAIT_TIME), + new WaitCommand(Constants.Auto.DriveToAmp.SNAPPING_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> 0.0, @@ -34,7 +34,7 @@ public DriveToAmpRed() { () -> false, () -> true)), new ParallelDeadlineGroup( - new WaitCommand(Constants.Auto.DriveToAmpRed.FORWARD_WAIT_TIME), + new WaitCommand(Constants.Auto.DriveToAmp.DRIVE_TO_AMP_TIME), new DriveFieldOrientedHeadingSnapping( () -> 0.0, () -> -1.0,