From: Arnav495 <arnieincyberland@gmail.com>
Date: Wed, 11 Mar 2026 21:41:53 +0000 (-0700)
Subject: Also check turret.
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=8f654276b5363e33a681432c23b1fad8f9ad68b5;p=FRC2026.git

Also check turret.
---

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 8257368..a131ebe 100644
--- 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 4f0107f..7427753 100644
--- 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 2c0e346..83137e3 100644
--- 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 33200ed..69486c3 100644
--- 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