Dont crash into trench -> master

src/main/java/frc/constants/field/Field.java

@@ -31,6 +31,8 @@ public static boolean isFieldConventionAlliance() {
 	public static final double DEPOT_Y_AXIS_LENGTH_METERS = 1.0668;
 	public static final double DEPOT_X_AXIS_LENGTH_METERS = 0.6858;
 	public static final double TOWER_Y_AXIS_LENGTH_METERS = 0.82;
+	public static final double TRENCH_Y_AXIS_LENGTH_METERS = 1.668;
+	public static final double TRENCH_BAR_X_AXIS_LENGTH_METERS = 0.101;
 
 	private static final Translation2d OUTPOST_MIDDLE = new Translation2d(0, 0.67);
 	public static final Translation2d TOWER_MIDDLE = new Translation2d(0.53, 3.75);

src/main/java/frc/robot/statemachine/ShootingCalculations.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.interpolation.InverseInterpolator;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import frc.constants.field.Field;
+import frc.robot.statemachine.shooterstatehandler.ShooterConstants;
 import frc.robot.statemachine.shooterstatehandler.ShootingParams;
 import frc.robot.subsystems.constants.hood.HoodConstants;
 import frc.robot.subsystems.constants.turret.TurretConstants;
@@ -21,6 +22,7 @@ public class ShootingCalculations {
 		HoodConstants.MINIMUM_POSITION,
 		TurretConstants.MIN_POSITION,
 		new Rotation2d(),
+		false,
 		new Translation2d(),
 		Field.getHubMiddle()
 	);
@@ -40,18 +42,12 @@ private static ShootingParams calculateShootingParams(
 		// Calculate distance from turret to target
 		Translation2d fieldRelativeTurretTranslation = getFieldRelativeTurretPosition(robotPose);
 		double distanceFromTurretToTargetMeters = targetTranslation.getDistance(fieldRelativeTurretTranslation);
-		// Split Robot's Speeds
-		Translation2d robotTranslationalVelocity = new Translation2d(
-			fieldRelativeSpeeds.vxMetersPerSecond,
-			fieldRelativeSpeeds.vyMetersPerSecond
-		);
 
-		// Turret Field Relative Velocity
-		Translation2d turretTangentialVelocity = TurretConstants.TURRET_POSITION_RELATIVE_TO_ROBOT.toTranslation2d()
-			.rotateBy(Rotation2d.kCCW_90deg)
-			.times(gyroYawAngularVelocity.getRadians())
-			.rotateBy(robotPose.getRotation());
-		Translation2d turretFieldRelativeVelocity = robotTranslationalVelocity.plus(turretTangentialVelocity);
+		Translation2d turretFieldRelativeVelocity = calculateFieldRelativeTurretVelocities(
+			robotPose,
+			fieldRelativeSpeeds,
+			gyroYawAngularVelocity
+		);
 
 		Translation2d turretPredictedPose = getPredictedTurretPose(
 			fieldRelativeTurretTranslation,
@@ -73,10 +69,21 @@ private static ShootingParams calculateShootingParams(
 		Rotation2d hoodTargetPosition = hoodInterpolation.get(distanceFromTurretPredictedPoseToHub);
 		Rotation2d flywheelTargetRPS = flywheelInterpolation.get(distanceFromTurretPredictedPoseToHub);
 
+		boolean isHoodTargetCorrect = true;
+
+		if (
+			ShootingChecks.isHoodInDangerOfCrushing(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity)
+				&& hoodTargetPosition.getDegrees() < ShooterConstants.MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH.getDegrees()
+		) {
+			hoodTargetPosition = ShooterConstants.MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH;
+			isHoodTargetCorrect = false;
+		}
+
 		Logger.recordOutput(LOG_PATH + "/turretFieldRelativePose", new Pose2d(fieldRelativeTurretTranslation, new Rotation2d()));
 		Logger.recordOutput(LOG_PATH + "/turretTarget", turretTargetPosition);
 		Logger.recordOutput(LOG_PATH + "/turretTargetVelocityRPS", turretTargetVelocityRPS);
 		Logger.recordOutput(LOG_PATH + "/hoodTarget", hoodTargetPosition);
+		Logger.recordOutput(LOG_PATH + "/isHoodTargetCorrect", isHoodTargetCorrect);
 		Logger.recordOutput(LOG_PATH + "/flywheelTarget", flywheelTargetRPS);
 		Logger.recordOutput(LOG_PATH + "/distanceFromTarget", distanceFromTurretToTargetMeters);
 		Logger.recordOutput(LOG_PATH + "/distanceFromTargetPredict", distanceFromTurretPredictedPoseToHub);
@@ -88,6 +95,7 @@ private static ShootingParams calculateShootingParams(
 			hoodTargetPosition,
 			turretTargetPosition,
 			turretTargetVelocityRPS,
+			isHoodTargetCorrect,
 			turretPredictedPose,
 			targetTranslation
 		);
@@ -123,6 +131,25 @@ private static ShootingParams calculatePassingParams(
 		);
 	}
 
+	public static Translation2d calculateFieldRelativeTurretVelocities(
+		Pose2d robotPose,
+		ChassisSpeeds fieldRelativeSpeeds,
+		Rotation2d gyroYawAngularVelocity
+	) {
+		// Split Robot's Speeds
+		Translation2d robotTranslationalVelocity = new Translation2d(
+			fieldRelativeSpeeds.vxMetersPerSecond,
+			fieldRelativeSpeeds.vyMetersPerSecond
+		);
+
+		// Turret Field Relative Velocity
+		Translation2d turretTangentialVelocity = TurretConstants.TURRET_POSITION_RELATIVE_TO_ROBOT.toTranslation2d()
+			.rotateBy(Rotation2d.kCCW_90deg)
+			.times(gyroYawAngularVelocity.getRadians())
+			.rotateBy(robotPose.getRotation());
+		return robotTranslationalVelocity.plus(turretTangentialVelocity);
+	}
+
 	private static Translation2d getPredictedTurretPoseByFlightTime(
 		Translation2d turretPose,
 		Translation2d turretVelocities,
@@ -166,6 +193,16 @@ public static Translation2d getFieldRelativeTurretPosition(Pose2d robotPose) {
 		);
 	}
 
+	public static Translation2d getTurretPositionWhenHoodCloses(
+		Translation2d currentTurretPosition,
+		Translation2d fieldRelativeTurretVelocities
+	) {
+		return new Translation2d(
+			currentTurretPosition.getX() + fieldRelativeTurretVelocities.getX() * ShooterConstants.TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC,
+			currentTurretPosition.getY() + fieldRelativeTurretVelocities.getY() * ShooterConstants.TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC
+		);
+	}
+
 	public static double getDistanceFromHub(Translation2d pose) {
 		return Field.getHubMiddle().getDistance(pose);
 	}

src/main/java/frc/robot/statemachine/ShootingChecks.java

@@ -1,12 +1,17 @@
 package frc.robot.statemachine;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc.constants.field.AllianceSide;
 import frc.constants.field.Field;
 import frc.robot.Robot;
 import frc.robot.statemachine.shooterstatehandler.ShooterConstants;
 import frc.utils.HubUtil;
+import frc.utils.math.AngleTransform;
+import frc.utils.math.FieldMath;
 import frc.utils.time.TimeUtil;
 import org.littletonrobotics.junction.Logger;
 
@@ -78,6 +83,61 @@ private static boolean isHoodAtPosition(Rotation2d hoodPosition, Rotation2d targ
 		return isHoodAtPosition;
 	}
 
+	private static boolean isTurretPositionAlignedWithTrenchOnYAxis(Translation2d allianceRelativeTurretPosition) {
+		return allianceRelativeTurretPosition.getY()
+			<= Field.getTrenchMiddle(AllianceSide.OUTPOST).getY() + (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2)
+			|| allianceRelativeTurretPosition.getY()
+				>= Field.getTrenchMiddle(AllianceSide.DEPOT).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2);
+	}
+
+	private static Boolean isTurretUnderTrench(Translation2d allianceRelativeTurretPosition) {
+		return (MathUtil.isNear(
+			allianceRelativeTurretPosition.getX(),
+			Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX(),
+			ShooterConstants.DISTANCH_FROM_TRENCH_CENTER_TO_CLOSE_HOOD
+		) && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition));
+	}
+
+	private static Boolean isRobotGoingUnderTrench(Pose2d robotPose, Translation2d fieldRelativeTurretVelocities) {
+		Boolean isPredictedTurretPoseUnderTrench;
+		Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose);
+		Translation2d allianceRelativePredictedTurretPositionWhenHoodCloses = ShootingCalculations
+			.getTurretPositionWhenHoodCloses(currentTurretPosition, fieldRelativeTurretVelocities);
+		Translation2d allianceRelativeOutpostTrenchMiddle = Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.OUTPOST));
+		Translation2d allianceRelativeDepotTrenchMiddle = Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT));
+		Translation2d allianceRelativeTurretPosition = Field.getAllianceRelative(currentTurretPosition);
+
+		boolean isTurretUnderTrench = isTurretUnderTrench(allianceRelativeTurretPosition);
+		boolean DoesTurretAlignWithTrenchOnYAxisUntilHoodCloses = isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)
+			|| isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses);
+
+		if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) {
+			isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX()
+				> allianceRelativeDepotTrenchMiddle.getX()
+				&& DoesTurretAlignWithTrenchOnYAxisUntilHoodCloses;
+		} else {
+			isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX()
+				< allianceRelativeDepotTrenchMiddle.getX()
+				&& DoesTurretAlignWithTrenchOnYAxisUntilHoodCloses;
+		}
+		Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isPredictedTurretPoseUnderTrench);
+		Logger.recordOutput(
+			shootingChecksLogPath + "/allianceRelativePredictedTurretPositionWhenHoodCloses",
+			new Pose2d(allianceRelativePredictedTurretPositionWhenHoodCloses, Rotation2d.k180deg)
+		);
+		return isPredictedTurretPoseUnderTrench || isTurretUnderTrench;
+	}
+
+	public static Boolean isHoodInDangerOfCrushing(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) {
+		Translation2d fieldRelativeTurretVelocities = ShootingCalculations
+			.calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity);
+		Boolean isRobotHeadingTowardsOurAllianceTrench = isRobotGoingUnderTrench(robotPose, fieldRelativeTurretVelocities);
+		Pose2d robotPoseReversed = FieldMath.mirror(robotPose, true, true, AngleTransform.INVERT);
+		Translation2d fieldRelativeTurretVelocitiesReversed = new Translation2d().minus(fieldRelativeTurretVelocities);
+		Boolean isRobotHeadingTowardsOpposingAllianceTrench = isRobotGoingUnderTrench(robotPoseReversed, fieldRelativeTurretVelocitiesReversed);
+		return (isRobotHeadingTowardsOurAllianceTrench || isRobotHeadingTowardsOpposingAllianceTrench);
+	}
+
 	private static boolean isReadyToShoot(
 		Robot robot,
 		Rotation2d flywheelVelocityToleranceRPS,
@@ -118,7 +178,13 @@ private static boolean isReadyToShoot(
 			logPath
 		);
 
-		boolean isReadyToShoot = isAtTurretAtTarget && isFlywheelReadyToShoot && isHoodAtPosition && isTurretWithinDistance
+		boolean isHoodPositionCorrect = ShootingCalculations.getShootingParams().isTargetHoodPositionCorrect();
+
+		boolean isReadyToShoot = isAtTurretAtTarget
+			&& isFlywheelReadyToShoot
+			&& isHoodAtPosition
+			&& isTurretWithinDistance
+			&& isHoodPositionCorrect
 		/* && isPoseReliable */;
 		Logger.recordOutput(shootingChecksLogPath + "/IsReadyToShoot", isReadyToShoot);
 
@@ -159,10 +225,13 @@ private static boolean canContinueShooting(
 		);
 		boolean isTurretWithinDistance = isWithinDistance(predictedTurretPosition, maxShootingDistanceFromTargetMeters, target, logPath);
 
+		boolean isHoodPositionCorrect = ShootingCalculations.getShootingParams().isTargetHoodPositionCorrect();
+
 		boolean canContinueShooting = isAtTurretAtTarget
 			&& isFlywheelReadyToShoot
 			&& isHoodAtPosition
-			&& isTurretWithinDistance /* && isPoseReliable */;
+			&& isTurretWithinDistance
+			&& isHoodPositionCorrect/* && isPoseReliable */;
 		Logger.recordOutput(shootingChecksLogPath + "/CanContinueShooting", canContinueShooting);
 
 		return canContinueShooting;

src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java

@@ -1,12 +1,18 @@
 package frc.robot.statemachine.shooterstatehandler;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import frc.constants.field.Field;
 import frc.utils.LoggedNetworkRotation2d;
 
 public class ShooterConstants {
 
 	public static final Rotation2d DEFAULT_FLYWHEEL_ROTATIONS_PER_SECOND = Rotation2d.fromRotations(10);
 
+	public static final double TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC = 0.5;
+	public static final double DISTANCH_FROM_TRENCH_CENTER_TO_CLOSE_HOOD = Field.TRENCH_BAR_X_AXIS_LENGTH_METERS * 2;
+
+	public static final Rotation2d MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH = Rotation2d.fromDegrees(50);
+
 	public static final LoggedNetworkRotation2d turretCalibrationAngle = new LoggedNetworkRotation2d(
 		"Tunable/TurretAngle",
 		Rotation2d.fromDegrees(0.0)

src/main/java/frc/robot/statemachine/shooterstatehandler/ShootingParams.java

@@ -9,6 +9,7 @@ public record ShootingParams(
 	Rotation2d targetHoodPosition,
 	Rotation2d targetTurretPosition,
 	Rotation2d targetTurretVelocityRPS,
+	boolean isTargetHoodPositionCorrect,
 	Translation2d predictedTurretPoseWhenBallLands,
 	Translation2d targetLandingPosition
 ) {}

src/main/java/frc/robot/subsystems/constants/hood/HoodCalibration.md

@@ -15,6 +15,7 @@ Hood calibrations
 - [x] Min position
 - [x] Forward limit
 - [x] Backwards limit
+- [ ] Min angle for trench
 
 #### FeedForward
 - [x] kS
@@ -38,4 +39,5 @@ Hood calibrations
 - [x] Current threshold to reset
 
 #### General Constants
-- [x] Arm length
+- [x] Arm length
+- [ ] Time to close hood