diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fb871347..0f496d5b 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. */ @@ -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 = 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; @@ -123,6 +123,33 @@ 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; + + /** 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; + + /** Time in seconds that we wait for TaxiLongAuto to end. */ + public static final double TAXI_LONG_WAIT_TIME = 2.0; + + /** 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 towards the amp. */ + public static final double DRIVE_TO_AMP_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; + } } /** Constants for the Intake System */ @@ -161,10 +188,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; @@ -175,7 +202,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 +260,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; @@ -273,6 +300,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 */ } } diff --git a/src/main/java/frc/robot/Controllers.java b/src/main/java/frc/robot/Controllers.java index 647b395c..292a1885 100644 --- a/src/main/java/frc/robot/Controllers.java +++ b/src/main/java/frc/robot/Controllers.java @@ -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.driveFieldOrientedSprint); keymapEntry.setString("Operatorless"); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9258b5d1..3efc3e27 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; @@ -29,9 +31,14 @@ 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; +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; @@ -59,6 +66,7 @@ import frc.robot.subsystems.mailbox.Mailbox; import frc.robot.subsystems.mailbox.MailboxBelts; import frc.robot.subsystems.mailbox.MailboxPneumatics; +import java.util.Optional; /** Singleton class that contains all the robot's subsystems, commands, and button bindings. */ @SuppressWarnings("unused") @@ -261,9 +269,25 @@ 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. */ 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(); @@ -273,7 +297,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. */ @@ -310,7 +334,7 @@ public RobotContainer() { startIntakeCamera.schedule(); - drive.setDefaultCommand(driveFieldOrientedHeadingSnapping); + drive.setDefaultCommand(driveFieldOriented); robotLights.setDefaultCommand(enabledLights); compressor.setDefaultCommand(controlCompressor); } @@ -328,6 +352,18 @@ 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("WORKING ONE PIECE AUTO I HOPE", onePieceAutoButItWorksISwear); + + autoChooser.addOption("LONG taxi auto", taxiLongAuto); + + autoChooser.addOption("Pick Up Note From Center", pickUpFromCenterAuto); + + autoChooser.addOption("NO INTAKE drive to center", driveToCenterAuto); + + autoChooser.addOption("Taxi with 10 second delay", delayedTaxiAuto); + Shuffleboard.getTab("Driver").add("Choose Auto Routine", autoChooser); } @@ -354,4 +390,18 @@ private void configureBindings() { public Command getAutonomousCommand() { return autoChooser.getSelected(); } + + /** + * Returns true if the robot is on the red alliance. False otherwise. + * + * @return True if the robot is on the red alliance. False otherwise. + */ + public static boolean isRedAlliance() { + Optional alliance = DriverStation.getAlliance(); + if (!alliance.isEmpty()) { + return alliance.get() == Alliance.Red; + } else { + return false; + } + } } diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java index c04d3c8d..6a72a98a 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), getRotation(), false); + drive.driveRobot(new Translation2d(getX(), getRotation()), 0, false); /* End the command if we're at our "aimed" threshold */ if (isAngled() && isDistanced()) { diff --git a/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java b/src/main/java/frc/robot/commands/SeekTargetWithLimelight.java index 81e41549..923d1508 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,8 +51,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.isRedAlliance()) { this.rotationRadiansPerSecond = rotationRadiansPerSecond * -1; } } 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 00000000..2676f415 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DelayedTaxiAuto.java @@ -0,0 +1,24 @@ +// 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() { + addCommands(new WaitCommand(Constants.Auto.TAXI_DELAY_TIME), new TaxiAuto()); + } +} 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 00000000..22444a3c --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpBlue.java @@ -0,0 +1,47 @@ +// 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.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; +import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; + +/** Auto that manually drives bot to blue amp. Made due to faulty limelight at comp. */ +public class DriveToAmpBlue extends SequentialCommandGroup { + + /** Creates a new DriveToAmpBlue. */ + public DriveToAmpBlue() { + super( + new ParallelDeadlineGroup( + new WaitCommand(Constants.Auto.DriveToAmp.SNAPPING_TIME), + new DriveFieldOrientedHeadingSnapping( + () -> 0.0, + () -> 0.0, + () -> 0.0, + () -> false, + () -> false, + () -> true, + () -> false)), + new ParallelDeadlineGroup( + new WaitCommand(Constants.Auto.DriveToAmp.DRIVE_TO_AMP_TIME), + new DriveFieldOrientedHeadingSnapping( + () -> 0.0, + () -> 1.0, + () -> 0.0, + () -> false, + () -> false, + () -> false, + () -> 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 00000000..a542120a --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DriveToAmpRed.java @@ -0,0 +1,47 @@ +// 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.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; +import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; + +/** Auto that manually drives bot to red amp. Made due to faulty limelight at comp. */ +public class DriveToAmpRed extends SequentialCommandGroup { + + /** Creates a new DriveToAmpRed. */ + public DriveToAmpRed() { + super( + new ParallelDeadlineGroup( + new WaitCommand(Constants.Auto.DriveToAmp.SNAPPING_TIME), + new DriveFieldOrientedHeadingSnapping( + () -> 0.0, + () -> 0.0, + () -> 0.0, + () -> false, + () -> false, + () -> false, + () -> true)), + new ParallelDeadlineGroup( + new WaitCommand(Constants.Auto.DriveToAmp.DRIVE_TO_AMP_TIME), + new DriveFieldOrientedHeadingSnapping( + () -> 0.0, + () -> -1.0, + () -> 0.0, + () -> false, + () -> false, + () -> false, + () -> false))); + } +} 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 00000000..3db0b1d3 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DriveToCenterAuto.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.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; + +/** Auto that drives bot to center of field. */ +public class DriveToCenterAuto extends ParallelCommandGroup { + /** Creates a new PickUpFromCenterAuto. */ + public DriveToCenterAuto() { + addCommands( + new ParallelDeadlineGroup( + new WaitCommand(Constants.Auto.DriveToCenter.DRIVE_CENTER_WAIT_TIME), + new DriveFieldOrientedHeadingSnapping( + () -> 1.0, + () -> 0.0, + () -> 0.0, + () -> false, + () -> true, + () -> false, + () -> false))); + } +} 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 00000000..89772023 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DumpNote.java @@ -0,0 +1,25 @@ +// 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.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(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 new file mode 100644 index 00000000..35bae2e3 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/OnePieceAutoButItWorksISwear.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.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotContainer; + +// One piece - the anime thing +/** + * 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() { + + if (RobotContainer.isRedAlliance()) { + 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 new file mode 100644 index 00000000..ed09b495 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/PickUpFromCenterAuto.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.Constants; +import frc.robot.commands.RunIntake; +import frc.robot.commands.drive.DriveFieldOrientedHeadingSnapping; + +/** Auto that drives the bot to a note and intakes it. */ +public class PickUpFromCenterAuto extends ParallelCommandGroup { + /** Creates a new PickUpFromCenterAuto. */ + public PickUpFromCenterAuto() { + + addCommands( + new RunIntake(), + new ParallelDeadlineGroup( + new WaitCommand(Constants.Auto.PICKUP_CENTER_WAIT_TIME), + new DriveFieldOrientedHeadingSnapping( + () -> 1.0, + () -> 0.0, + () -> 0.0, + () -> false, + () -> true, + () -> false, + () -> false))); + } +} 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 00000000..88220796 --- /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(Constants.Auto.TAXI_LONG_WAIT_TIME)); + addCommands( + new DriveAutoRobotOriented( + new Translation2d(Constants.Auto.TAXI_AUTO_METERS_PER_SECOND, 0), 0)); + } +} diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java index b731dfea..3a98f3e6 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. diff --git a/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java b/src/main/java/frc/robot/commands/drive/SetDrivePerspectiveFieldOrientedHeadingSnapping.java index 61faf250..f5d79c54 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. diff --git a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java index 5f3bbe7b..bc1af952 100644 --- a/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java +++ b/src/main/java/frc/robot/commands/lightstrip/EnabledLight.java @@ -11,13 +11,10 @@ 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 +29,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.isRedAlliance()) { + 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/AddressableLightStrip.java b/src/main/java/frc/robot/subsystems/AddressableLightStrip.java index 0d1bdaa0..e9a05717 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()) { diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index fea140cd..1c21d494 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -19,12 +19,12 @@ 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; 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 +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 */ - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; + return RobotContainer.isRedAlliance(); }, this /* Reference to this subsystem to set requirements */); }