GKC 2024 Changes (Disaster)

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,42 @@ 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 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.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;
+    }
+
+    /** 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 +197,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 +211,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 +269,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 +309,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 */
   }
 }

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");
   }

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> alliance = DriverStation.getAlliance();
+    if (!alliance.isEmpty()) {
+      return alliance.get() == Alliance.Red;
+    } else {
+      return false;
+    }
+  }
 }

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()) {

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;
     }
   }

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());
+  }
+}

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.DriveToAmpBlue.LEFT_WAIT_TIME),
+            new DriveFieldOrientedHeadingSnapping(
+                () -> 0.0,
+                () -> 0.0,
+                () -> 0.0,
+                () -> false,
+                () -> false,
+                () -> true,
+                () -> false)),
+        new ParallelDeadlineGroup(
+            new WaitCommand(Constants.Auto.DriveToAmpBlue.FORWARD_WAIT_TIME),
+            new DriveFieldOrientedHeadingSnapping(
+                () -> 0.0,
+                () -> 1.0,
+                () -> 0.0,
+                () -> false,
+                () -> false,
+                () -> false,
+                () -> false)));
+  }
+}

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.DriveToAmpRed.LEFT_WAIT_TIME),
+            new DriveFieldOrientedHeadingSnapping(
+                () -> 0.0,
+                () -> 0.0,
+                () -> 0.0,
+                () -> false,
+                () -> false,
+                () -> false,
+                () -> true)),
+        new ParallelDeadlineGroup(
+            new WaitCommand(Constants.Auto.DriveToAmpRed.FORWARD_WAIT_TIME),
+            new DriveFieldOrientedHeadingSnapping(
+                () -> 0.0,
+                () -> -1.0,
+                () -> 0.0,
+                () -> false,
+                () -> false,
+                () -> false,
+                () -> false)));
+  }
+}