Also check turret.

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 8257368323f7844e931c42f07ae4850499dd6f4a..a131ebefb008025540c9b9876eca89185679eab4 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -153,8 +153,8 @@ public class RobotContainer {
         break;
     }
 
-       if (intake != null && hood != null)
-               CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake));
+       if (intake != null && hood != null && turret != null)
+               CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret));
 
     // 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
index 4f0107f68dcc654c0b409efcb8fb4532c3cecec4..74277536ee5ada2cd94396be4751f6141e0b2d39 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/HardstopWarning.java
+++ b/src/main/java/frc/robot/commands/gpm/HardstopWarning.java
@@ -6,14 +6,19 @@ import frc.robot.constants.IntakeConstants;
 import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.hood.Hood;
 import frc.robot.subsystems.hood.HoodConstants;
+import frc.robot.subsystems.turret.Turret;
 
 public class HardstopWarning extends Command {
        private Hood hood;
        private Intake intake;
+       private Turret turret;
+       private String turretStatus;
 
-       public HardstopWarning(Hood hood, Intake intake) {
+       public HardstopWarning(Hood hood, Intake intake, Turret turret) {
                this.hood = hood;
                this.intake = intake;
+               this.turret = turret;
+               turretStatus = "Unknown";
        }
 
        @Override
@@ -26,6 +31,16 @@ public class HardstopWarning extends Command {
                double epsilon = 0.05;
                SmartDashboard.putBoolean("Hood OK", hood.getPositionDeg() >= HoodConstants.MIN_ANGLE - epsilon);
                SmartDashboard.putBoolean("Intake OK", intake.getPosition() >= IntakeConstants.STARTING_POINT - epsilon);
+
+               if (Math.abs(turret.getPositionRad()) <= epsilon) {
+                       var encoderPositions = turret.getEncoderPositions();
+                       if (Math.abs(encoderPositions.getFirst()) <= epsilon && Math.abs(encoderPositions.getSecond()) <= epsilon)
+                               turretStatus = "Ok";
+                       else
+                               turretStatus = "Bad";
+               }
+
+               SmartDashboard.putString("Turret Status", turretStatus);
        }
 
        @Override

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 2c0e346d38feb76b82e5dc595b27c81e295571c4..83137e3809c9df717c2a4af544409e04d79b96dd 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -12,9 +12,10 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.sim.TalonFXSimState;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
@@ -316,4 +317,11 @@ public class Turret extends SubsystemBase implements TurretIO{
                calibrating = false;
                setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0);
        }
+
+       // in rotations
+       public Pair<Double, Double> getEncoderPositions() {
+               return new Pair<Double, Double>(
+                               encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET,
+                               encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET);
+       }
 }

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index 33200edf3125070b10d465d2e6a6370531543d59..69486c390b1abd7a9bf9ddd32047a6be87789374 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -23,6 +23,7 @@ public class TurretConstants {
     public static int RIGHT_ENCODER_TEETH = 22; // read above
     public static int ENCODER_COUNT_TOTAL = 8192; // how many intervals it can have, like clicks on a clock chat gpt explained to me
 
+       // TODO: set these properly
     public static double LEFT_ENCODER_OFFSET = 0.364502; //  rot
     public static double RIGHT_ENCODER_OFFSET = 0.718506; //  rot