From 4475508c59330ddab44133394ff533f4c0dae01c Mon Sep 17 00:00:00 2001 From: tomer Date: Thu, 4 Jun 2026 18:40:39 +0300 Subject: [PATCH 01/20] to run robot --- .../statemachine/ShootingCalculations.java | 27 +++++++++++-------- .../robot/statemachine/ShootingChecks.java | 9 +++++++ 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index e3e59f4b4b..31d6e6d8bd 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/src/main/java/frc/robot/statemachine/ShootingCalculations.java @@ -40,18 +40,8 @@ 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, @@ -123,6 +113,21 @@ 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, diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index f9cfc45720..c51f6c0e5c 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -1,10 +1,13 @@ 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.Field; import frc.robot.Robot; +import frc.robot.hardware.digitalinput.channeled.ChanneledDigitalInput; import frc.robot.statemachine.shooterstatehandler.ShooterConstants; import frc.utils.HubUtil; import frc.utils.time.TimeUtil; @@ -78,6 +81,12 @@ private static boolean isHoodAtPosition(Rotation2d hoodPosition, Rotation2d targ return isHoodAtPosition; } + private static boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity){ + Translation2d fieldRelativeTurretVelocities = ShootingCalculations.calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); + Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); + Translation2d predictedTurretPositionWhenHoodCloses = new Translation2d(currentTurretPosition.getX()+fieldRelativeTurretVelocities.getX()*,); + } + private static boolean isReadyToShoot( Robot robot, Rotation2d flywheelVelocityToleranceRPS, From 095a8177851a2daeeb96251516fa9dc2a7a67297 Mon Sep 17 00:00:00 2001 From: tomer Date: Sun, 7 Jun 2026 15:53:48 +0300 Subject: [PATCH 02/20] made one func --- src/main/java/frc/constants/field/Field.java | 1 + src/main/java/frc/robot/statemachine/ShootingChecks.java | 9 +++++++-- .../shooterstatehandler/ShooterConstants.java | 2 ++ 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/constants/field/Field.java b/src/main/java/frc/constants/field/Field.java index 1da6c8b565..23febdc489 100644 --- a/src/main/java/frc/constants/field/Field.java +++ b/src/main/java/frc/constants/field/Field.java @@ -31,6 +31,7 @@ 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; private static final Translation2d OUTPOST_MIDDLE = new Translation2d(0, 0.67); public static final Translation2d TOWER_MIDDLE = new Translation2d(0.53, 3.75); diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index c51f6c0e5c..a142bfbd4a 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -5,9 +5,9 @@ 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.hardware.digitalinput.channeled.ChanneledDigitalInput; import frc.robot.statemachine.shooterstatehandler.ShooterConstants; import frc.utils.HubUtil; import frc.utils.time.TimeUtil; @@ -84,7 +84,12 @@ private static boolean isHoodAtPosition(Rotation2d hoodPosition, Rotation2d targ private static boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity){ Translation2d fieldRelativeTurretVelocities = ShootingCalculations.calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); - Translation2d predictedTurretPositionWhenHoodCloses = new Translation2d(currentTurretPosition.getX()+fieldRelativeTurretVelocities.getX()*,); + Translation2d predictedAllianceRelativeTurretPositionWhenHoodCloses = Field.getAllianceRelative(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)); + if(Field.getAllianceRelative(currentTurretPosition).getX() < Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX()){ + return ((predictedAllianceRelativeTurretPositionWhenHoodCloses.getX()>Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX())&&((predictedAllianceRelativeTurretPositionWhenHoodCloses.getY()>Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getY()-(Field.TRENCH_Y_AXIS_LENGTH_METERS/2))||(predictedAllianceRelativeTurretPositionWhenHoodCloses.getY()Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getY()-(Field.TRENCH_Y_AXIS_LENGTH_METERS/2))||(predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() Date: Thu, 11 Jun 2026 11:20:38 +0300 Subject: [PATCH 03/20] befor chacking --- src/main/java/frc/constants/field/Field.java | 1 + src/main/java/frc/robot/Robot.java | 6 +++ .../statemachine/ShootingCalculations.java | 22 +++++++--- .../robot/statemachine/ShootingChecks.java | 44 ++++++++++++++++--- .../shooterstatehandler/ShooterConstants.java | 2 + 5 files changed, 61 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/constants/field/Field.java b/src/main/java/frc/constants/field/Field.java index 23febdc489..3733fbec95 100644 --- a/src/main/java/frc/constants/field/Field.java +++ b/src/main/java/frc/constants/field/Field.java @@ -32,6 +32,7 @@ public static boolean isFieldConventionAlliance() { 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); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 472879609f..d95bf7c6a1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,7 +24,9 @@ import frc.robot.statemachine.RobotCommander; import frc.robot.statemachine.RobotState; import frc.robot.statemachine.ShootingCalculations; +import frc.robot.statemachine.ShootingChecks; import frc.robot.statemachine.intakestatehandler.IntakeState; +import frc.robot.statemachine.shooterstatehandler.ShooterConstants; import frc.robot.subsystems.arm.CurrentControlArm; import frc.robot.subsystems.arm.VelocityPositionArm; import frc.robot.poseestimator.IPoseEstimator; @@ -306,6 +308,10 @@ public Robot() { configureBrakeStateChooser(); configureAuto(); + new Trigger( + () -> (ShootingChecks + .areWeGonnaDoGA(poseEstimator.getEstimatedPose(), swerve.getFieldRelativeVelocity(), swerve.getIMUAngularVelocityRPS()[2])) + ).whileTrue(hood.getCommandsBuilder().setTargetPosition(ShooterConstants.MIN_HOOD_POSITION_FOR_PASSING_TRENCH)); } public RobotConfig getRobotConfig() { diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index 31d6e6d8bd..07ceefc449 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/src/main/java/frc/robot/statemachine/ShootingCalculations.java @@ -41,7 +41,11 @@ private static ShootingParams calculateShootingParams( Translation2d fieldRelativeTurretTranslation = getFieldRelativeTurretPosition(robotPose); double distanceFromTurretToTargetMeters = targetTranslation.getDistance(fieldRelativeTurretTranslation); - Translation2d turretFieldRelativeVelocity = calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); + Translation2d turretFieldRelativeVelocity = calculateFieldRelativeTurretVelocities( + robotPose, + fieldRelativeSpeeds, + gyroYawAngularVelocity + ); Translation2d turretPredictedPose = getPredictedTurretPose( fieldRelativeTurretTranslation, @@ -113,18 +117,22 @@ private static ShootingParams calculatePassingParams( ); } - public static Translation2d calculateFieldRelativeTurretVelocities(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity){ + public static Translation2d calculateFieldRelativeTurretVelocities( + Pose2d robotPose, + ChassisSpeeds fieldRelativeSpeeds, + Rotation2d gyroYawAngularVelocity + ) { // Split Robot's Speeds Translation2d robotTranslationalVelocity = new Translation2d( - fieldRelativeSpeeds.vxMetersPerSecond, - fieldRelativeSpeeds.vyMetersPerSecond + 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()); + .rotateBy(Rotation2d.kCCW_90deg) + .times(gyroYawAngularVelocity.getRadians()) + .rotateBy(robotPose.getRotation()); return robotTranslationalVelocity.plus(turretTangentialVelocity); } diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index a142bfbd4a..d3574b6fa6 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -81,15 +81,45 @@ private static boolean isHoodAtPosition(Rotation2d hoodPosition, Rotation2d targ return isHoodAtPosition; } - private static boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity){ - Translation2d fieldRelativeTurretVelocities = ShootingCalculations.calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); + public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { + Boolean areWeTryingToGoUnderTrench; + Translation2d fieldRelativeTurretVelocities = ShootingCalculations + .calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); - Translation2d predictedAllianceRelativeTurretPositionWhenHoodCloses = Field.getAllianceRelative(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)); - if(Field.getAllianceRelative(currentTurretPosition).getX() < Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX()){ - return ((predictedAllianceRelativeTurretPositionWhenHoodCloses.getX()>Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX())&&((predictedAllianceRelativeTurretPositionWhenHoodCloses.getY()>Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getY()-(Field.TRENCH_Y_AXIS_LENGTH_METERS/2))||(predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX()) + && ((predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() + > Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2)) + || (predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() + < Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.OUTPOST)).getY() + + (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2))); + } else { + areWeTryingToGoUnderTrench = ((predictedAllianceRelativeTurretPositionWhenHoodCloses.getX() + < Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX()) + && ((predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() + > Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2)) + || (predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() + < Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.OUTPOST)).getY() + + (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2)))); } - return ((predictedAllianceRelativeTurretPositionWhenHoodCloses.getX()Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getY()-(Field.TRENCH_Y_AXIS_LENGTH_METERS/2))||(predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() Date: Wed, 17 Jun 2026 14:19:25 +0300 Subject: [PATCH 04/20] tomer - made main func cleaner --- .../robot/statemachine/ShootingChecks.java | 43 +++++++++---------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index d3574b6fa6..1864b2ecd0 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -81,42 +81,39 @@ private static boolean isHoodAtPosition(Rotation2d hoodPosition, Rotation2d targ return isHoodAtPosition; } + private static boolean isPositionAlignedWithTrenchOnTAxis(Translation2d allianceRelativeTurretPosition) { + return allianceRelativeTurretPosition.getY() + <= Field.getTrenchMiddle(AllianceSide.OUTPOST).getY() + Field.TRENCH_Y_AXIS_LENGTH_METERS / 2 + || allianceRelativeTurretPosition.getY() + >= Field.getTrenchMiddle(AllianceSide.OUTPOST).getY() - Field.TRENCH_Y_AXIS_LENGTH_METERS / 2; + } + public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { Boolean areWeTryingToGoUnderTrench; Translation2d fieldRelativeTurretVelocities = ShootingCalculations .calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); - Translation2d predictedAllianceRelativeTurretPositionWhenHoodCloses = Field.getAllianceRelative( + Translation2d allianceRelativePredictedTurretPositionWhenHoodCloses = Field.getAllianceRelative( 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 ) ); - boolean isTurretUnderTrench = MathUtil.isNear( - Field.getAllianceRelative(currentTurretPosition).getX(), - Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX(), - Field.TRENCH_BAR_X_AXIS_LENGTH_METERS / 2 - ); + Translation2d allianceRelativeOutpostTrenchMiddle = Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.OUTPOST)); + Translation2d allianceRelativeDepotTrenchMiddle = Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)); + Translation2d allianceRelativeTurretPosition = Field.getAllianceRelative(currentTurretPosition); + + boolean isTurretUnderTrench = (MathUtil + .isNear(allianceRelativeTurretPosition.getX(), allianceRelativeDepotTrenchMiddle.getX(), Field.TRENCH_BAR_X_AXIS_LENGTH_METERS / 2) + && isPositionAlignedWithTrenchOnTAxis(allianceRelativeTurretPosition)); if (isTurretUnderTrench) { areWeTryingToGoUnderTrench = true; - } else if ( - Field.getAllianceRelative(currentTurretPosition).getX() < Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX() - ) { - areWeTryingToGoUnderTrench = (predictedAllianceRelativeTurretPositionWhenHoodCloses.getX() - > Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX()) - && ((predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() - > Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2)) - || (predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() - < Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.OUTPOST)).getY() - + (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2))); + } else if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) { + areWeTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() > allianceRelativeDepotTrenchMiddle.getX() + && isPositionAlignedWithTrenchOnTAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } else { - areWeTryingToGoUnderTrench = ((predictedAllianceRelativeTurretPositionWhenHoodCloses.getX() - < Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX()) - && ((predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() - > Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2)) - || (predictedAllianceRelativeTurretPositionWhenHoodCloses.getY() - < Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.OUTPOST)).getY() - + (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2)))); + areWeTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() + && isPositionAlignedWithTrenchOnTAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", areWeTryingToGoUnderTrench); return areWeTryingToGoUnderTrench; From b429b7e3b0a1b69325f5ecf8d5b39bf17dfc1342 Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 17 Jun 2026 14:41:55 +0300 Subject: [PATCH 05/20] tomer - my codde not coding --- .../robot/statemachine/ShootingChecks.java | 24 +++++++++---------- .../shooterstatehandler/ShooterConstants.java | 2 +- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index 1864b2ecd0..66009619cd 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -81,15 +81,15 @@ private static boolean isHoodAtPosition(Rotation2d hoodPosition, Rotation2d targ return isHoodAtPosition; } - private static boolean isPositionAlignedWithTrenchOnTAxis(Translation2d allianceRelativeTurretPosition) { + private static boolean isPositionAlignedWithTrenchOnYAxis(Translation2d allianceRelativeTurretPosition) { return allianceRelativeTurretPosition.getY() - <= Field.getTrenchMiddle(AllianceSide.OUTPOST).getY() + Field.TRENCH_Y_AXIS_LENGTH_METERS / 2 + <= Field.getTrenchMiddle(AllianceSide.OUTPOST).getY() + (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2) || allianceRelativeTurretPosition.getY() - >= Field.getTrenchMiddle(AllianceSide.OUTPOST).getY() - Field.TRENCH_Y_AXIS_LENGTH_METERS / 2; + >= Field.getTrenchMiddle(AllianceSide.OUTPOST).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2); } public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { - Boolean areWeTryingToGoUnderTrench; + Boolean isRobotTryingToGoUnderTrench; Translation2d fieldRelativeTurretVelocities = ShootingCalculations .calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); @@ -105,18 +105,18 @@ public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelati boolean isTurretUnderTrench = (MathUtil .isNear(allianceRelativeTurretPosition.getX(), allianceRelativeDepotTrenchMiddle.getX(), Field.TRENCH_BAR_X_AXIS_LENGTH_METERS / 2) - && isPositionAlignedWithTrenchOnTAxis(allianceRelativeTurretPosition)); + && isPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); if (isTurretUnderTrench) { - areWeTryingToGoUnderTrench = true; + isRobotTryingToGoUnderTrench = true; } else if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) { - areWeTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() > allianceRelativeDepotTrenchMiddle.getX() - && isPositionAlignedWithTrenchOnTAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); + isRobotTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() > allianceRelativeDepotTrenchMiddle.getX() + && isPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } else { - areWeTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() - && isPositionAlignedWithTrenchOnTAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); + isRobotTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() + && isPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } - Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", areWeTryingToGoUnderTrench); - return areWeTryingToGoUnderTrench; + Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isRobotTryingToGoUnderTrench); + return isRobotTryingToGoUnderTrench; } private static boolean isReadyToShoot( diff --git a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java index db93ba8dc5..845a2d2355 100644 --- a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java +++ b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java @@ -7,7 +7,7 @@ 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.1; + public static final Double TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC = 1.0; public static final Rotation2d MIN_HOOD_POSITION_FOR_PASSING_TRENCH = Rotation2d.fromDegrees(50); From 8aa95102ba07596197c73ca46715c5d333ffd89a Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 17 Jun 2026 18:07:35 +0300 Subject: [PATCH 06/20] tomer - to pull master --- src/main/java/frc/robot/Robot.java | 17 ++++++++++---- .../robot/statemachine/ShootingChecks.java | 22 ++++++++++++------- .../shooterstatehandler/ShooterConstants.java | 2 +- 3 files changed, 28 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fbdb09724f..8f982f7e5a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -311,10 +311,8 @@ public Robot() { configureBrakeStateChooser(); configureAuto(); - new Trigger( - () -> (ShootingChecks - .areWeGonnaDoGA(poseEstimator.getEstimatedPose(), swerve.getFieldRelativeVelocity(), swerve.getIMUAngularVelocityRPS()[2])) - ).whileTrue(hood.getCommandsBuilder().setTargetPosition(ShooterConstants.MIN_HOOD_POSITION_FOR_PASSING_TRENCH)); + + goUnderTrenchTrigger(); } public RobotConfig getRobotConfig() { @@ -398,6 +396,17 @@ public double getAverageBPSForLastXSeconds(double seconds) { return 0; } + private void goUnderTrenchTrigger(){ + new Trigger( + () -> (ShootingChecks + .areWeGonnaDoGA(poseEstimator.getEstimatedPose(), swerve.getFieldRelativeVelocity(), swerve.getIMUAngularVelocityRPS()[2])) + ).whileTrue( + new ParallelCommandGroup( + new InstantCommand(() -> hood.setIsRunningIndependently(true)), + hood.getCommandsBuilder().setTargetPosition(ShooterConstants.MIN_HOOD_POSITION_FOR_PASSING_TRENCH) + ).andThen(new InstantCommand(() -> hood.setIsRunningIndependently(false))) + );} + public FlyWheel getFlyWheel() { return flyWheel; } diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index 66009619cd..aed06b6917 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -81,11 +81,11 @@ private static boolean isHoodAtPosition(Rotation2d hoodPosition, Rotation2d targ return isHoodAtPosition; } - private static boolean isPositionAlignedWithTrenchOnYAxis(Translation2d allianceRelativeTurretPosition) { + 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.OUTPOST).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2); + >= Field.getTrenchMiddle(AllianceSide.DEPOT).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2); } public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { @@ -104,18 +104,24 @@ public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelati Translation2d allianceRelativeTurretPosition = Field.getAllianceRelative(currentTurretPosition); boolean isTurretUnderTrench = (MathUtil - .isNear(allianceRelativeTurretPosition.getX(), allianceRelativeDepotTrenchMiddle.getX(), Field.TRENCH_BAR_X_AXIS_LENGTH_METERS / 2) - && isPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); + .isNear(allianceRelativeTurretPosition.getX(), allianceRelativeDepotTrenchMiddle.getX(), Field.TRENCH_BAR_X_AXIS_LENGTH_METERS * 2) + && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); if (isTurretUnderTrench) { isRobotTryingToGoUnderTrench = true; } else if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) { - isRobotTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() > allianceRelativeDepotTrenchMiddle.getX() - && isPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); + isRobotTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() + > allianceRelativeDepotTrenchMiddle.getX() + && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } else { - isRobotTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() - && isPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); + isRobotTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() + < allianceRelativeDepotTrenchMiddle.getX() + && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isRobotTryingToGoUnderTrench); + Logger.recordOutput( + shootingChecksLogPath + "/allianceRelativePredictedTurretPositionWhenHoodCloses", + new Pose2d(allianceRelativePredictedTurretPositionWhenHoodCloses, Rotation2d.k180deg) + ); return isRobotTryingToGoUnderTrench; } diff --git a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java index 845a2d2355..4aa42a67d1 100644 --- a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java +++ b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java @@ -7,7 +7,7 @@ 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 = 1.0; + public static final Double TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC = 0.5; public static final Rotation2d MIN_HOOD_POSITION_FOR_PASSING_TRENCH = Rotation2d.fromDegrees(50); From e3c029dc97b461f7fb1b8ad68167416ed5bb03fb Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 17 Jun 2026 18:09:53 +0300 Subject: [PATCH 07/20] tomer - sA --- src/main/java/frc/robot/Robot.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 22f8e790e9..67c531c2fc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -396,16 +396,17 @@ public double getAverageBPSForLastXSeconds(double seconds) { return 0; } - private void goUnderTrenchTrigger(){ + private void goUnderTrenchTrigger() { new Trigger( () -> (ShootingChecks - .areWeGonnaDoGA(poseEstimator.getEstimatedPose(), swerve.getFieldRelativeVelocity(), swerve.getIMUAngularVelocityRPS()[2])) - ).whileTrue( + .areWeGonnaDoGA(poseEstimator.getEstimatedPose(), swerve.getFieldRelativeVelocity(), swerve.getIMUAngularVelocityRPS()[2])) + ).whileTrue( new ParallelCommandGroup( - new InstantCommand(() -> hood.setIsRunningIndependently(true)), - hood.getCommandsBuilder().setTargetPosition(ShooterConstants.MIN_HOOD_POSITION_FOR_PASSING_TRENCH) + new InstantCommand(() -> hood.setIsRunningIndependently(true)), + hood.getCommandsBuilder().setTargetPosition(ShooterConstants.MIN_HOOD_POSITION_FOR_PASSING_TRENCH) ).andThen(new InstantCommand(() -> hood.setIsRunningIndependently(false))) - );} + ); + } public void enableAllLimelightTemperatureRegulations() { this.limelightFront.setThrottleState(true); From a54451342be85901c902c233eed690047c3bf3b6 Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 17 Jun 2026 18:17:16 +0300 Subject: [PATCH 08/20] tomer - self CR --- src/main/java/frc/robot/Robot.java | 2 +- .../statemachine/shooterstatehandler/ShooterConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 67c531c2fc..acae074999 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -403,7 +403,7 @@ private void goUnderTrenchTrigger() { ).whileTrue( new ParallelCommandGroup( new InstantCommand(() -> hood.setIsRunningIndependently(true)), - hood.getCommandsBuilder().setTargetPosition(ShooterConstants.MIN_HOOD_POSITION_FOR_PASSING_TRENCH) + hood.getCommandsBuilder().setTargetPosition(ShooterConstants.MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH) ).andThen(new InstantCommand(() -> hood.setIsRunningIndependently(false))) ); } diff --git a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java index 4aa42a67d1..11fbc624e3 100644 --- a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java +++ b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java @@ -9,7 +9,7 @@ public class ShooterConstants { public static final Double TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC = 0.5; - public static final Rotation2d MIN_HOOD_POSITION_FOR_PASSING_TRENCH = Rotation2d.fromDegrees(50); + public static final Rotation2d MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH = Rotation2d.fromDegrees(50); public static final LoggedNetworkRotation2d turretCalibrationAngle = new LoggedNetworkRotation2d( "Tunable/TurretAngle", From 419f9c3086e54e33741a67b6ca0f9c90cb1bdc6b Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 17 Jun 2026 21:14:01 +0300 Subject: [PATCH 09/20] tomer - me idion nitay goat --- src/main/java/frc/robot/Robot.java | 14 -------------- .../frc/robot/statemachine/RobotCommander.java | 2 ++ .../robot/statemachine/ShootingCalculations.java | 1 + 3 files changed, 3 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index acae074999..238f84b8fb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -311,8 +311,6 @@ public Robot() { configureBrakeStateChooser(); configureAuto(); - - goUnderTrenchTrigger(); } public RobotConfig getRobotConfig() { @@ -396,18 +394,6 @@ public double getAverageBPSForLastXSeconds(double seconds) { return 0; } - private void goUnderTrenchTrigger() { - new Trigger( - () -> (ShootingChecks - .areWeGonnaDoGA(poseEstimator.getEstimatedPose(), swerve.getFieldRelativeVelocity(), swerve.getIMUAngularVelocityRPS()[2])) - ).whileTrue( - new ParallelCommandGroup( - new InstantCommand(() -> hood.setIsRunningIndependently(true)), - hood.getCommandsBuilder().setTargetPosition(ShooterConstants.MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH) - ).andThen(new InstantCommand(() -> hood.setIsRunningIndependently(false))) - ); - } - public void enableAllLimelightTemperatureRegulations() { this.limelightFront.setThrottleState(true); this.limelightRight.setThrottleState(true); diff --git a/src/main/java/frc/robot/statemachine/RobotCommander.java b/src/main/java/frc/robot/statemachine/RobotCommander.java index 2b2e77ec7a..fd11e9c574 100644 --- a/src/main/java/frc/robot/statemachine/RobotCommander.java +++ b/src/main/java/frc/robot/statemachine/RobotCommander.java @@ -1,11 +1,13 @@ package frc.robot.statemachine; import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.statemachine.funnelstatehandler.FunnelState; import frc.robot.statemachine.funnelstatehandler.FunnelStateHandler; import frc.robot.statemachine.intakestatehandler.IntakeState; import frc.robot.statemachine.intakestatehandler.IntakeStateHandler; +import frc.robot.statemachine.shooterstatehandler.ShooterConstants; import frc.robot.statemachine.shooterstatehandler.ShooterState; import frc.robot.statemachine.shooterstatehandler.ShooterStateHandler; import frc.robot.subsystems.GBSubsystem; diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index 8b71f4a11c..006f235d4b 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/src/main/java/frc/robot/statemachine/ShootingCalculations.java @@ -67,6 +67,7 @@ private static ShootingParams calculateShootingParams( Rotation2d hoodTargetPosition = hoodInterpolation.get(distanceFromTurretPredictedPoseToHub); Rotation2d flywheelTargetRPS = flywheelInterpolation.get(distanceFromTurretPredictedPoseToHub); + if(ShootingChecks.areWeGonnaDoGA(robotPose,fieldRelativeSpeeds, gyroYawAngularVelocity)) Logger.recordOutput(LOG_PATH + "/turretFieldRelativePose", new Pose2d(fieldRelativeTurretTranslation, new Rotation2d())); Logger.recordOutput(LOG_PATH + "/turretTarget", turretTargetPosition); Logger.recordOutput(LOG_PATH + "/turretTargetVelocityRPS", turretTargetVelocityRPS); From 8d29863970c92897a519d6adc02fb376bfd31477 Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 17 Jun 2026 21:15:10 +0300 Subject: [PATCH 10/20] tomer - sA --- src/main/java/frc/robot/Robot.java | 2 -- src/main/java/frc/robot/statemachine/RobotCommander.java | 2 -- .../java/frc/robot/statemachine/ShootingCalculations.java | 4 ++-- 3 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 238f84b8fb..f79cce7bb8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,9 +24,7 @@ import frc.robot.statemachine.RobotCommander; import frc.robot.statemachine.RobotState; import frc.robot.statemachine.ShootingCalculations; -import frc.robot.statemachine.ShootingChecks; import frc.robot.statemachine.intakestatehandler.IntakeState; -import frc.robot.statemachine.shooterstatehandler.ShooterConstants; import frc.robot.subsystems.arm.CurrentControlArm; import frc.robot.subsystems.arm.VelocityPositionArm; import frc.robot.poseestimator.IPoseEstimator; diff --git a/src/main/java/frc/robot/statemachine/RobotCommander.java b/src/main/java/frc/robot/statemachine/RobotCommander.java index fd11e9c574..2b2e77ec7a 100644 --- a/src/main/java/frc/robot/statemachine/RobotCommander.java +++ b/src/main/java/frc/robot/statemachine/RobotCommander.java @@ -1,13 +1,11 @@ package frc.robot.statemachine; import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.statemachine.funnelstatehandler.FunnelState; import frc.robot.statemachine.funnelstatehandler.FunnelStateHandler; import frc.robot.statemachine.intakestatehandler.IntakeState; import frc.robot.statemachine.intakestatehandler.IntakeStateHandler; -import frc.robot.statemachine.shooterstatehandler.ShooterConstants; import frc.robot.statemachine.shooterstatehandler.ShooterState; import frc.robot.statemachine.shooterstatehandler.ShooterStateHandler; import frc.robot.subsystems.GBSubsystem; diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index 006f235d4b..22793d74f1 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/src/main/java/frc/robot/statemachine/ShootingCalculations.java @@ -67,8 +67,8 @@ private static ShootingParams calculateShootingParams( Rotation2d hoodTargetPosition = hoodInterpolation.get(distanceFromTurretPredictedPoseToHub); Rotation2d flywheelTargetRPS = flywheelInterpolation.get(distanceFromTurretPredictedPoseToHub); - if(ShootingChecks.areWeGonnaDoGA(robotPose,fieldRelativeSpeeds, gyroYawAngularVelocity)) - Logger.recordOutput(LOG_PATH + "/turretFieldRelativePose", new Pose2d(fieldRelativeTurretTranslation, new Rotation2d())); + if (ShootingChecks.areWeGonnaDoGA(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity)) + 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); From 3ad66754b22a9c795db1940002dc9a8ac0fe921c Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 17 Jun 2026 21:24:04 +0300 Subject: [PATCH 11/20] tomer - i stupid --- .../java/frc/robot/statemachine/ShootingCalculations.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index 22793d74f1..706dc6adec 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/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; @@ -67,8 +68,10 @@ private static ShootingParams calculateShootingParams( Rotation2d hoodTargetPosition = hoodInterpolation.get(distanceFromTurretPredictedPoseToHub); Rotation2d flywheelTargetRPS = flywheelInterpolation.get(distanceFromTurretPredictedPoseToHub); - if (ShootingChecks.areWeGonnaDoGA(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity)) - Logger.recordOutput(LOG_PATH + "/turretFieldRelativePose", new Pose2d(fieldRelativeTurretTranslation, new Rotation2d())); + if (ShootingChecks.areWeGonnaDoGA(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity)) { + hoodTargetPosition = ShooterConstants.MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH; + } + 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); From d9ae8dfbf8aab0d1df3f4799a03dff79c0d3dc80 Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 17 Jun 2026 21:26:34 +0300 Subject: [PATCH 12/20] tomer - format --- src/main/java/frc/robot/statemachine/ShootingCalculations.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index 706dc6adec..f21b6f302f 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/src/main/java/frc/robot/statemachine/ShootingCalculations.java @@ -71,6 +71,7 @@ private static ShootingParams calculateShootingParams( if (ShootingChecks.areWeGonnaDoGA(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity)) { hoodTargetPosition = ShooterConstants.MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH; } + Logger.recordOutput(LOG_PATH + "/turretFieldRelativePose", new Pose2d(fieldRelativeTurretTranslation, new Rotation2d())); Logger.recordOutput(LOG_PATH + "/turretTarget", turretTargetPosition); Logger.recordOutput(LOG_PATH + "/turretTargetVelocityRPS", turretTargetVelocityRPS); From 101a2d2fe37bf6d494d4b8252e5136c6d12069ce Mon Sep 17 00:00:00 2001 From: tomer Date: Sat, 20 Jun 2026 17:31:51 +0300 Subject: [PATCH 13/20] tomer - some more logic --- .../robot/statemachine/ShootingCalculations.java | 11 ++++++++++- .../frc/robot/statemachine/ShootingChecks.java | 14 ++++++++++++-- .../shooterstatehandler/ShootingParams.java | 1 + 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index f21b6f302f..36a57126b8 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/src/main/java/frc/robot/statemachine/ShootingCalculations.java @@ -22,6 +22,7 @@ public class ShootingCalculations { HoodConstants.MINIMUM_POSITION, TurretConstants.MIN_POSITION, new Rotation2d(), + false, new Translation2d(), Field.getHubMiddle() ); @@ -68,14 +69,21 @@ private static ShootingParams calculateShootingParams( Rotation2d hoodTargetPosition = hoodInterpolation.get(distanceFromTurretPredictedPoseToHub); Rotation2d flywheelTargetRPS = flywheelInterpolation.get(distanceFromTurretPredictedPoseToHub); - if (ShootingChecks.areWeGonnaDoGA(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity)) { + boolean isHoodTargetCorrect = true; + + if ( + ShootingChecks.areWeGonnaDoGA(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); @@ -87,6 +95,7 @@ private static ShootingParams calculateShootingParams( hoodTargetPosition, turretTargetPosition, turretTargetVelocityRPS, + isHoodTargetCorrect, turretPredictedPose, targetTranslation ); diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index aed06b6917..cfab0ad326 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -165,7 +165,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); @@ -206,10 +212,14 @@ 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; diff --git a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShootingParams.java b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShootingParams.java index 2e8ede11a6..82a7415ec4 100644 --- a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShootingParams.java +++ b/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 ) {} From b0587d0a0d895f1f93445155d3c0190e6180acab Mon Sep 17 00:00:00 2001 From: tomer Date: Sun, 21 Jun 2026 16:29:03 +0300 Subject: [PATCH 14/20] tomer - mereged master --- .../frc/robot/statemachine/ShootingChecks.java | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index cfab0ad326..188b622bcb 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -88,10 +88,8 @@ private static boolean isTurretPositionAlignedWithTrenchOnYAxis(Translation2d al >= Field.getTrenchMiddle(AllianceSide.DEPOT).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2); } - public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { + private static Boolean isRobotHeadingTowardsTrench(Pose2d robotPose, Translation2d fieldRelativeTurretVelocities) { Boolean isRobotTryingToGoUnderTrench; - Translation2d fieldRelativeTurretVelocities = ShootingCalculations - .calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); Translation2d allianceRelativePredictedTurretPositionWhenHoodCloses = Field.getAllianceRelative( new Translation2d( @@ -125,6 +123,19 @@ public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelati return isRobotTryingToGoUnderTrench; } + public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { + Translation2d fieldRelativeTurretVelocities = ShootingCalculations + .calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); + Boolean isRobotHeadingTowardsOurAllianceTrench = isRobotHeadingTowardsTrench(robotPose, fieldRelativeTurretVelocities); + Pose2d robotPoseReversed = Field.getAllianceRelative(robotPose); + Translation2d fieldRelativeTurretVelocitiesReversed = new Translation2d().minus(fieldRelativeTurretVelocities); + Boolean isRobotHeadingTowardsOpposingAllianceTrench = isRobotHeadingTowardsTrench( + robotPoseReversed, + fieldRelativeTurretVelocitiesReversed + ); + return (isRobotHeadingTowardsOurAllianceTrench || isRobotHeadingTowardsOpposingAllianceTrench); + } + private static boolean isReadyToShoot( Robot robot, Rotation2d flywheelVelocityToleranceRPS, From 84a4aa093190a0eab1770c4a028b1e0596472a8b Mon Sep 17 00:00:00 2001 From: tomer Date: Mon, 22 Jun 2026 09:29:37 +0300 Subject: [PATCH 15/20] tomer - dolev CR --- .../statemachine/ShootingCalculations.java | 9 ++++- .../robot/statemachine/ShootingChecks.java | 35 ++++++++++--------- 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index 36a57126b8..32149fbc34 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/src/main/java/frc/robot/statemachine/ShootingCalculations.java @@ -72,7 +72,7 @@ private static ShootingParams calculateShootingParams( boolean isHoodTargetCorrect = true; if ( - ShootingChecks.areWeGonnaDoGA(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity) + 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; @@ -193,6 +193,13 @@ 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); } diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index 188b622bcb..5ff4e8d4ac 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -10,6 +10,8 @@ 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; @@ -88,46 +90,45 @@ private static boolean isTurretPositionAlignedWithTrenchOnYAxis(Translation2d al >= 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(), Field.TRENCH_BAR_X_AXIS_LENGTH_METERS * 2) + && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); + } + private static Boolean isRobotHeadingTowardsTrench(Pose2d robotPose, Translation2d fieldRelativeTurretVelocities) { - Boolean isRobotTryingToGoUnderTrench; + Boolean isRobotPredictedUnderTrench; Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); - Translation2d allianceRelativePredictedTurretPositionWhenHoodCloses = Field.getAllianceRelative( - 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 - ) - ); + 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 = (MathUtil - .isNear(allianceRelativeTurretPosition.getX(), allianceRelativeDepotTrenchMiddle.getX(), Field.TRENCH_BAR_X_AXIS_LENGTH_METERS * 2) - && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); + boolean isTurretUnderTrench = isTurretUnderTrench(allianceRelativeTurretPosition); if (isTurretUnderTrench) { - isRobotTryingToGoUnderTrench = true; + isRobotPredictedUnderTrench = true; } else if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) { - isRobotTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() + isRobotPredictedUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() > allianceRelativeDepotTrenchMiddle.getX() && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } else { - isRobotTryingToGoUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() + isRobotPredictedUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } - Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isRobotTryingToGoUnderTrench); + Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isRobotPredictedUnderTrench); Logger.recordOutput( shootingChecksLogPath + "/allianceRelativePredictedTurretPositionWhenHoodCloses", new Pose2d(allianceRelativePredictedTurretPositionWhenHoodCloses, Rotation2d.k180deg) ); - return isRobotTryingToGoUnderTrench; + return isRobotPredictedUnderTrench; } - public static Boolean areWeGonnaDoGA(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { + public static Boolean isHoodInDangerOfCrushing(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { Translation2d fieldRelativeTurretVelocities = ShootingCalculations .calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); Boolean isRobotHeadingTowardsOurAllianceTrench = isRobotHeadingTowardsTrench(robotPose, fieldRelativeTurretVelocities); - Pose2d robotPoseReversed = Field.getAllianceRelative(robotPose); + Pose2d robotPoseReversed = FieldMath.mirror(robotPose,true,true, AngleTransform.INVERT); Translation2d fieldRelativeTurretVelocitiesReversed = new Translation2d().minus(fieldRelativeTurretVelocities); Boolean isRobotHeadingTowardsOpposingAllianceTrench = isRobotHeadingTowardsTrench( robotPoseReversed, From c2549c6f828ef3a63372e051b843bc6127d6dc46 Mon Sep 17 00:00:00 2001 From: tomer Date: Mon, 22 Jun 2026 09:31:51 +0300 Subject: [PATCH 16/20] tomer - sA --- .../statemachine/ShootingCalculations.java | 9 +++++--- .../robot/statemachine/ShootingChecks.java | 21 ++++++++++--------- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index 32149fbc34..be1914b19a 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/src/main/java/frc/robot/statemachine/ShootingCalculations.java @@ -193,10 +193,13 @@ public static Translation2d getFieldRelativeTurretPosition(Pose2d robotPose) { ); } - public static Translation2d getTurretPositionWhenHoodCloses(Translation2d currentTurretPosition, Translation2d fieldRelativeTurretVelocities){ + 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 + currentTurretPosition.getX() + fieldRelativeTurretVelocities.getX() * ShooterConstants.TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC, + currentTurretPosition.getY() + fieldRelativeTurretVelocities.getY() * ShooterConstants.TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC ); } diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index 5ff4e8d4ac..be9ad315fb 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -90,16 +90,19 @@ private static boolean isTurretPositionAlignedWithTrenchOnYAxis(Translation2d al >= 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(), Field.TRENCH_BAR_X_AXIS_LENGTH_METERS * 2) - && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); + private static Boolean isTurretUnderTrench(Translation2d allianceRelativeTurretPosition) { + return (MathUtil.isNear( + allianceRelativeTurretPosition.getX(), + Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX(), + Field.TRENCH_BAR_X_AXIS_LENGTH_METERS * 2 + ) && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); } private static Boolean isRobotHeadingTowardsTrench(Pose2d robotPose, Translation2d fieldRelativeTurretVelocities) { Boolean isRobotPredictedUnderTrench; Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); - Translation2d allianceRelativePredictedTurretPositionWhenHoodCloses = ShootingCalculations.getTurretPositionWhenHoodCloses(currentTurretPosition, fieldRelativeTurretVelocities); + 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); @@ -108,12 +111,10 @@ private static Boolean isRobotHeadingTowardsTrench(Pose2d robotPose, Translation if (isTurretUnderTrench) { isRobotPredictedUnderTrench = true; } else if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) { - isRobotPredictedUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() - > allianceRelativeDepotTrenchMiddle.getX() + isRobotPredictedUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() > allianceRelativeDepotTrenchMiddle.getX() && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } else { - isRobotPredictedUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() - < allianceRelativeDepotTrenchMiddle.getX() + isRobotPredictedUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isRobotPredictedUnderTrench); @@ -128,7 +129,7 @@ public static Boolean isHoodInDangerOfCrushing(Pose2d robotPose, ChassisSpeeds f Translation2d fieldRelativeTurretVelocities = ShootingCalculations .calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); Boolean isRobotHeadingTowardsOurAllianceTrench = isRobotHeadingTowardsTrench(robotPose, fieldRelativeTurretVelocities); - Pose2d robotPoseReversed = FieldMath.mirror(robotPose,true,true, AngleTransform.INVERT); + Pose2d robotPoseReversed = FieldMath.mirror(robotPose, true, true, AngleTransform.INVERT); Translation2d fieldRelativeTurretVelocitiesReversed = new Translation2d().minus(fieldRelativeTurretVelocities); Boolean isRobotHeadingTowardsOpposingAllianceTrench = isRobotHeadingTowardsTrench( robotPoseReversed, From b780d7d3204e2c1b3e8606b1d2723a2b720b582c Mon Sep 17 00:00:00 2001 From: tomer Date: Tue, 23 Jun 2026 11:57:42 +0300 Subject: [PATCH 17/20] tomer - project got scraped --- .../robot/statemachine/ShootingChecks.java | 24 +++++++++---------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index be9ad315fb..055d015fdd 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -98,8 +98,8 @@ private static Boolean isTurretUnderTrench(Translation2d allianceRelativeTurretP ) && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); } - private static Boolean isRobotHeadingTowardsTrench(Pose2d robotPose, Translation2d fieldRelativeTurretVelocities) { - Boolean isRobotPredictedUnderTrench; + private static Boolean isRobotGoingUnderTrench(Pose2d robotPose, Translation2d fieldRelativeTurretVelocities) { + Boolean isPredictedTurretPoseUnderTrench; Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); Translation2d allianceRelativePredictedTurretPositionWhenHoodCloses = ShootingCalculations .getTurretPositionWhenHoodCloses(currentTurretPosition, fieldRelativeTurretVelocities); @@ -108,30 +108,29 @@ private static Boolean isRobotHeadingTowardsTrench(Pose2d robotPose, Translation Translation2d allianceRelativeTurretPosition = Field.getAllianceRelative(currentTurretPosition); boolean isTurretUnderTrench = isTurretUnderTrench(allianceRelativeTurretPosition); - if (isTurretUnderTrench) { - isRobotPredictedUnderTrench = true; - } else if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) { - isRobotPredictedUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() > allianceRelativeDepotTrenchMiddle.getX() + + if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) { + isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() > allianceRelativeDepotTrenchMiddle.getX() && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } else { - isRobotPredictedUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() + isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } - Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isRobotPredictedUnderTrench); + Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isPredictedTurretPoseUnderTrench); Logger.recordOutput( - shootingChecksLogPath + "/allianceRelativePredictedTurretPositionWhenHoodCloses", + shootingChecksLogPath + "/allianceRelativePredictedTurretPositionWhenHoodCloses", new Pose2d(allianceRelativePredictedTurretPositionWhenHoodCloses, Rotation2d.k180deg) ); - return isRobotPredictedUnderTrench; + return isPredictedTurretPoseUnderTrench; } public static Boolean isHoodInDangerOfCrushing(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { Translation2d fieldRelativeTurretVelocities = ShootingCalculations .calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); - Boolean isRobotHeadingTowardsOurAllianceTrench = isRobotHeadingTowardsTrench(robotPose, fieldRelativeTurretVelocities); + Boolean isRobotHeadingTowardsOurAllianceTrench = isRobotGoingUnderTrench(robotPose, fieldRelativeTurretVelocities); Pose2d robotPoseReversed = FieldMath.mirror(robotPose, true, true, AngleTransform.INVERT); Translation2d fieldRelativeTurretVelocitiesReversed = new Translation2d().minus(fieldRelativeTurretVelocities); - Boolean isRobotHeadingTowardsOpposingAllianceTrench = isRobotHeadingTowardsTrench( + Boolean isRobotHeadingTowardsOpposingAllianceTrench = isRobotGoingUnderTrench( robotPoseReversed, fieldRelativeTurretVelocitiesReversed ); @@ -227,7 +226,6 @@ private static boolean canContinueShooting( boolean isHoodPositionCorrect = ShootingCalculations.getShootingParams().isTargetHoodPositionCorrect(); - boolean canContinueShooting = isAtTurretAtTarget && isFlywheelReadyToShoot && isHoodAtPosition From be0a3268b32b731b9abb35665be9ddcfa22eaf17 Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 24 Jun 2026 11:23:32 +0300 Subject: [PATCH 18/20] tomer - sA --- .../frc/robot/statemachine/ShootingChecks.java | 15 +++++++-------- .../shooterstatehandler/ShooterConstants.java | 4 +++- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index 055d015fdd..28b090bb53 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -94,7 +94,7 @@ private static Boolean isTurretUnderTrench(Translation2d allianceRelativeTurretP return (MathUtil.isNear( allianceRelativeTurretPosition.getX(), Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX(), - Field.TRENCH_BAR_X_AXIS_LENGTH_METERS * 2 + ShooterConstants.DISTANCH_FROM_TRENCH_CENTER_TO_CLOSE_HOOD ) && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); } @@ -110,15 +110,17 @@ private static Boolean isRobotGoingUnderTrench(Pose2d robotPose, Translation2d f boolean isTurretUnderTrench = isTurretUnderTrench(allianceRelativeTurretPosition); if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) { - isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() > allianceRelativeDepotTrenchMiddle.getX() + isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() + > allianceRelativeDepotTrenchMiddle.getX() && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } else { - isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() + isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() + < allianceRelativeDepotTrenchMiddle.getX() && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); } Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isPredictedTurretPoseUnderTrench); Logger.recordOutput( - shootingChecksLogPath + "/allianceRelativePredictedTurretPositionWhenHoodCloses", + shootingChecksLogPath + "/allianceRelativePredictedTurretPositionWhenHoodCloses", new Pose2d(allianceRelativePredictedTurretPositionWhenHoodCloses, Rotation2d.k180deg) ); return isPredictedTurretPoseUnderTrench; @@ -130,10 +132,7 @@ public static Boolean isHoodInDangerOfCrushing(Pose2d robotPose, ChassisSpeeds f 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 - ); + Boolean isRobotHeadingTowardsOpposingAllianceTrench = isRobotGoingUnderTrench(robotPoseReversed, fieldRelativeTurretVelocitiesReversed); return (isRobotHeadingTowardsOurAllianceTrench || isRobotHeadingTowardsOpposingAllianceTrench); } diff --git a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java index 11fbc624e3..10cefe04ac 100644 --- a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java +++ b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java @@ -1,13 +1,15 @@ 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 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); From da69cb71d74f8aa125be8dcbe9a5142e11c27fc3 Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 24 Jun 2026 11:24:44 +0300 Subject: [PATCH 19/20] tomer - added something small --- src/main/java/frc/robot/statemachine/ShootingChecks.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index 28b090bb53..2452f5cd9e 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -123,7 +123,7 @@ private static Boolean isRobotGoingUnderTrench(Pose2d robotPose, Translation2d f shootingChecksLogPath + "/allianceRelativePredictedTurretPositionWhenHoodCloses", new Pose2d(allianceRelativePredictedTurretPositionWhenHoodCloses, Rotation2d.k180deg) ); - return isPredictedTurretPoseUnderTrench; + return isPredictedTurretPoseUnderTrench || isTurretUnderTrench; } public static Boolean isHoodInDangerOfCrushing(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { From 248da42389e66640599b05de2f1c9a69b56a22d0 Mon Sep 17 00:00:00 2001 From: tomer Date: Wed, 24 Jun 2026 11:34:56 +0300 Subject: [PATCH 20/20] tomer - fixed logic arror --- src/main/java/frc/robot/statemachine/ShootingChecks.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index 2452f5cd9e..206ea96fcc 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -108,15 +108,17 @@ private static Boolean isRobotGoingUnderTrench(Pose2d robotPose, Translation2d f 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() - && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); + && DoesTurretAlignWithTrenchOnYAxisUntilHoodCloses; } else { isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() < allianceRelativeDepotTrenchMiddle.getX() - && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); + && DoesTurretAlignWithTrenchOnYAxisUntilHoodCloses; } Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isPredictedTurretPoseUnderTrench); Logger.recordOutput(