From 6a9dee57d1b24de427d6cadc11c190fd69567acc Mon Sep 17 00:00:00 2001
From: Arnav495 <arnieincyberland@gmail.com>
Date: Tue, 10 Mar 2026 16:36:28 -0700
Subject: [PATCH] Add warning for things.

---
 src/main/java/frc/robot/RobotContainer.java   |  4 +++
 .../robot/commands/gpm/HardstopWarning.java   | 35 +++++++++++++++++++
 2 files changed, 39 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/gpm/HardstopWarning.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e736859..8257368 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -24,6 +24,7 @@ import frc.robot.commands.DoNothing;
 import frc.robot.commands.drive_comm.DefaultDriveCommand;
 import frc.robot.commands.gpm.AutoShootCommand;
 import frc.robot.commands.gpm.ClimbDriveCommand;
+import frc.robot.commands.gpm.HardstopWarning;
 import frc.robot.commands.gpm.IntakeMovementCommand;
 import frc.robot.commands.gpm.RunSpindexer;
 import frc.robot.commands.gpm.Superstructure;
@@ -152,6 +153,9 @@ public class RobotContainer {
         break;
     }
 
+	if (intake != null && hood != null)
+		CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake));
+
     // This is really annoying so it's disabled
     DriverStation.silenceJoystickConnectionWarning(true);
 
diff --git a/src/main/java/frc/robot/commands/gpm/HardstopWarning.java b/src/main/java/frc/robot/commands/gpm/HardstopWarning.java
new file mode 100644
index 0000000..6fa6b13
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/HardstopWarning.java
@@ -0,0 +1,35 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.IntakeConstants;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.hood.HoodConstants;
+
+public class HardstopWarning extends Command {
+	private Hood hood;
+	private Intake intake;
+
+	public HardstopWarning(Hood hood, Intake intake) {
+		this.hood = hood;
+		this.intake = intake;
+	}
+
+	@Override
+	public boolean runsWhenDisabled() {
+		return true;
+	}
+
+	@Override
+	public void execute() {
+		double epsilon = 0.05;
+		SmartDashboard.putBoolean("Hood OK", hood.getPositionDeg() < HoodConstants.MIN_ANGLE - epsilon);
+		SmartDashboard.putBoolean("Intake OK", intake.getPosition() < IntakeConstants.STARTING_POINT - epsilon);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}
-- 
2.39.5