From aa578bd9948725bb37f2d34a6330059d291c380e Mon Sep 17 00:00:00 2001 From: Bry <155580880+BCODERW@users.noreply.github.com> Date: Tue, 17 Mar 2026 15:16:47 -0400 Subject: [PATCH 1/3] Cleaned up superstructure #1 --- src/main/java/frc/robot/Superstructure.java | 33 +++++++++++++++------ 1 file changed, 24 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index e20119bb..d666763d 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -44,37 +44,52 @@ public Superstructure(Shooter shooter) { this.shooter = shooter; if(CURRENT_MODE == Mode.SIM) { shouldStowTrigger.whileTrue( - shooter.stowCommand()); + shooter.stowCommand() + ); + scoringModeTrigger.whileTrue( shooter.setSimParameters( () -> RobotContainer.aiming.calculateSimShot( HUB, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + ferryLeftTrigger.whileTrue( shooter.setSimParameters( () -> RobotContainer.aiming.calculateSimShot( FERRY_LEFT, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + ferryRightTrigger.whileTrue( shooter.setSimParameters( () -> RobotContainer.aiming.calculateSimShot( FERRY_RIGHT, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + } else { shouldStowTrigger.whileTrue( - shooter.stowCommand()); + shooter.stowCommand() + ); + scoringModeTrigger.whileTrue( shooter.setParameters( () -> Aiming.calculateShot(HUB, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + ferryLeftTrigger.whileTrue( shooter.setParameters( () -> Aiming.calculateShot(FERRY_LEFT, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); + ferryRightTrigger.whileTrue( shooter.setParameters( () -> Aiming.calculateShot(FERRY_RIGHT, shooter.withConstantVelocity, shooter.whileMoving) - )); + ) + ); } } @@ -143,7 +158,7 @@ public void resetTrenchZones() { public void updateTrenchZonesVeloBased() { //Updates width of zone based on robot velocity Distance dynamicTrenchDangerZoneWidth = Meters.of(TRENCH_ZONE_OFFSET.in(Meters) + Math.abs(RobotContainer.drivetrain.getFieldRelativeVelocity().getX())* HoodConstants.HOOD_LOWER_TIME); - FieldConstants.Zones.TRENCH_DANGER_ZONES = new Translation2d[][]{ + FieldConstants.Zones.TRENCH_DANGER_ZONES = new Translation2d[][] { // near right trench new Translation2d[] { new Translation2d(LinesVertical.HUB_CENTER.minus(dynamicTrenchDangerZoneWidth), Meters.zero()), From c2ea7c4361c253abc1c596619fd55f521c34fbde Mon Sep 17 00:00:00 2001 From: Bry <155580880+BCODERW@users.noreply.github.com> Date: Tue, 17 Mar 2026 15:24:39 -0400 Subject: [PATCH 2/3] Cleaned up superstructure #2 --- src/main/java/frc/robot/Superstructure.java | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index d666763d..c88ced0a 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -195,7 +195,7 @@ public static boolean isInScoringZone() { return (AllianceManager.isRed() ? pos.getX() >= PoseUtils.flipTranslationAlliance(new Translation2d(FieldConstants.LinesVertical.HUB_CENTER.in(Meters), 0)).getX() - : + : pos.getX() <= FieldConstants.LinesVertical.HUB_CENTER.in(Meters) ); } @@ -204,14 +204,13 @@ public boolean shouldBeScoringBasedOnZones() { //for now // if(!AllianceManager.isAllianceKnown() || (HubTracker.getCurrentShift().isEmpty())) return false; - if(!AllianceManager.isAllianceKnown()) return false; - - return !inTrenchDangerZone() && isInScoringZone(); //Are we in the scoring zone and is the hub active // return // !inTrenchDangerZone() // && isInScoringZone() // && HubTracker.isActive(DriverStation.getAlliance().get(), HubTracker.getCurrentShift().get()); + + return AllianceManager.isAllianceKnown() ? !inTrenchDangerZone() && isInScoringZone() : false; } @@ -263,9 +262,11 @@ public static boolean inTrenchZone() { && robotPose.getX() <= zone[1].getX() && robotPose.getY() >= zone[0].getY() && robotPose.getY() <= zone[1].getY()) { + return true; } } + return false; } @@ -277,9 +278,11 @@ public static boolean inTrenchDangerZone() { && turretPose.getX() <= zone[1].getX() && turretPose.getY() >= zone[0].getY() && turretPose.getY() <= zone[1].getY()) { + return true; } } + return false; } @@ -295,9 +298,11 @@ public static boolean inBumpZone() { && robotPose.getX() <= zone[1].getX() && robotPose.getY() >= zone[0].getY() && robotPose.getY() <= zone[1].getY()) { + return true; } } + return false; } @@ -319,7 +324,6 @@ public static boolean movingIntoObstacle() { || (robotPose.getX() > FieldConstants.LinesVertical.OPP_ALLIANCE_ZONE.minus(FieldConstants.Hub.WIDTH.div(2)).minus(Constants.BUMPER_WIDTH).in(Meters) && RobotContainer.drivetrain.getForwardVelocityFromController() < 0); - if(AllianceManager.isRed()) return !movingIntoObstacleOnBlue; - return movingIntoObstacleOnBlue; + return AllianceManager.isRed() ? !movingIntoObstacleOnBlue : movingIntoObstacleOnBlue; } } From 5b4079eb28e8e804e98daa35ff5184f2b5d4f980 Mon Sep 17 00:00:00 2001 From: Bry <155580880+BCODERW@users.noreply.github.com> Date: Tue, 17 Mar 2026 15:32:50 -0400 Subject: [PATCH 3/3] Superstructure cleanup #3 --- src/main/java/frc/robot/Superstructure.java | 29 ++++++++------------- 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index c88ced0a..431ac626 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -215,40 +215,31 @@ public boolean shouldBeScoringBasedOnZones() { public boolean shouldFerryLeft() { - if(!AllianceManager.isAllianceKnown()) return false; - Translation2d pos = RobotContainer.turret.getFieldRelativePosition(); + Translation2d pos = RobotContainer.turret.getFieldRelativePosition(); - return - !inTrenchDangerZone() && - ( - AllianceManager.isRed() ? + return !AllianceManager.isAllianceKnown() ? false : !inTrenchDangerZone() && (AllianceManager.isRed() ? ( pos.getY() <= PoseUtils.flipTranslationAlliance(new Translation2d(0, FieldConstants.LinesHorizontal.CENTER.in(Meters))).getY() && pos.getX() <= PoseUtils.flipTranslationAlliance(new Translation2d(FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters), 0)).getX() ) : ( - pos.getY() >= FieldConstants.LinesHorizontal.CENTER.in(Meters) - && pos.getX() >= FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters) + pos.getY() >= FieldConstants.LinesHorizontal.CENTER.in(Meters) && pos.getX() >= FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters) ) ); } public boolean shouldFerryRight() { - if(!AllianceManager.isAllianceKnown()) return false; - Translation2d pos = RobotContainer.turret.getFieldRelativePosition(); + Translation2d pos = RobotContainer.turret.getFieldRelativePosition(); - return - !inTrenchDangerZone() && - (AllianceManager.isRed() ? - ( + return !AllianceManager.isAllianceKnown() ? false : !inTrenchDangerZone() && (AllianceManager.isRed() ? + ( pos.getY() >= PoseUtils.flipTranslationAlliance(new Translation2d(0, FieldConstants.LinesHorizontal.CENTER.in(Meters))).getY() && pos.getX() <= PoseUtils.flipTranslationAlliance(new Translation2d(FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters), 0)).getX() ) - : + : ( - pos.getY() <= FieldConstants.LinesHorizontal.CENTER.in(Meters) - && pos.getX() >= FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters) + pos.getY() <= FieldConstants.LinesHorizontal.CENTER.in(Meters) && pos.getX() >= FieldConstants.LinesVertical.NEUTRAL_ZONE_NEAR.in(Meters) ) ); } @@ -309,9 +300,11 @@ public static boolean inBumpZone() { // are we moving INTO the trench? public static boolean movingIntoObstacle() { Pose2d robotPose = RobotContainer.drivetrain.getRobotPose(); + boolean movingIntoObstacleOnBlue = (robotPose.getX() < FieldConstants.LinesVertical.ALLIANCE_ZONE.plus(FieldConstants.Hub.WIDTH.div(2)).plus(Constants.BUMPER_WIDTH).in(Meters) - && RobotContainer.drivetrain.getForwardVelocityFromController() > 0 ) + + && RobotContainer.drivetrain.getForwardVelocityFromController() > 0) || (robotPose.getX() > FieldConstants.LinesVertical.ALLIANCE_ZONE.in(Meters) && robotPose.getX() < FieldConstants.LinesVertical.CENTER.in(Meters)