Add gyro set on houndbrian

frc868 · Mar 21, 2024 · 968e812 · 968e812
1 parent 756cfa4
commit 968e812
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/HoundBrian.java b/src/main/java/frc/robot/HoundBrian.java
@@ -47,6 +47,11 @@ public class HoundBrian {
     public HoundBrian(Drivetrain drivetrain, Intake intake, ShooterTilt shooterTilt, Climber climber, NoteLift noteLift,
             LEDs leds) {
 
+        new Trigger(drivetrainButton::get).negate()
+                // .and(new Trigger(intakeButton::get).negate().debounce(2,
+                // DebounceType.kFalling))
+                .and(DriverStation::isDisabled)
+                .onTrue(drivetrain.resetGyroCommand().ignoringDisable(true));
         new Trigger(intakeButton::get).negate()
                 // .and(new Trigger(intakeButton::get).negate().debounce(2,
                 // DebounceType.kFalling))

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -160,7 +160,8 @@ public RobotContainer() {
                         climber.resetControllersCommand()).withName("resetControllers"));
 
         new Trigger(() -> {
-            return intake.getInitialized()
+            return drivetrain.getInitialized()
+                    && intake.getInitialized()
                     && shooterTilt.getInitialized()
                     && climber.getInitialized()
                     && noteLift.getInitialized();
@@ -172,6 +173,7 @@ public RobotContainer() {
 
         leds.requestStateCommand(LEDState.INITIALIZATION_BLACK_BACKGROUND).until(GlobalStates.INITIALIZED::enabled)
                 .schedule();
+        leds.requestStateCommand(LEDState.DRIVETRAIN_GYRO_UNINITIALIZED).until(drivetrain::getInitialized).schedule();
         leds.requestStateCommand(LEDState.INTAKE_UNINITIALIZED).until(intake::getInitialized).schedule();
         leds.requestStateCommand(LEDState.SHOOTER_TILT_UNINITIALIZED).until(shooterTilt::getInitialized).schedule();
         leds.requestStateCommand(LEDState.CLIMBER_UNINITIALIZED).until(climber::getInitialized).schedule();

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -195,6 +195,8 @@ public class Drivetrain extends SubsystemBase implements BaseSwerveDrive {
     @Log
     private int failedDaqs = 0;
 
+    private boolean initialized = false;
+
     /** Initializes the drivetrain. */
     public Drivetrain() {
         poseEstimator = new SwerveDrivePoseEstimator(
@@ -574,7 +576,8 @@ public void resetPoseEstimator(Pose2d pose) {
 
     @Override
     public void resetGyro() {
-        pigeon.setYaw(0);
+        pigeon.setYaw(DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? 180 : 0);
+        initialized = true;
     }
 
     @Override
@@ -1074,4 +1077,7 @@ public Pose3d calculateEffectiveTargetLocation() {
                 getFieldRelativeAccelerations());
     }
 
+    public boolean getInitialized() {
+        return initialized;
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java
@@ -44,7 +44,8 @@ public enum LEDState {
                 breathe(Color.kRed, 3, 0, 255, LEDSection.SHOOTER_RIGHT_BOTTOM)),
         SHOOTER_TILT_UNINITIALIZED(
                 breathe(Color.kRed, 3, 0, 255, LEDSection.SHOOTER_LEFT_TOP),
-                breathe(Color.kRed, 3, 0, 255, LEDSection.SHOOTER_RIGHT_TOP),
+                breathe(Color.kRed, 3, 0, 255, LEDSection.SHOOTER_RIGHT_TOP)),
+        DRIVETRAIN_GYRO_UNINITIALIZED(
                 breathe(Color.kRed, 3, 0, 255, LEDSection.SHOOTER_TOP)),
         INITIALIZATION_BLACK_BACKGROUND(solid(Color.kBlack, LEDSection.ALL)),
         INITIALIZED_CONFIRM(breathe(Color.kGreen, 2, 0, 255, LEDSection.ALL)),