Add warning for things.

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e736859890d0cd1a631fa8c9cfa9f49c5998c856..8257368323f7844e931c42f07ae4850499dd6f4a 100644 (file)
--- 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 (file)
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;
+       }
+}