Skip to content

Cleaned up superstructure #1 #23

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
BCODERW wants to merge 3 commits into
base: main
Choose a base branch
from
+45 −33
Open

Cleaned up superstructure #1 #23

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
aa578bd
Cleaned up superstructure #1
BCODERW Mar 17, 2026
c2ea7c4
Cleaned up superstructure #2
BCODERW Mar 17, 2026
5b4079e
Superstructure cleanup #3
BCODERW Mar 17, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
78 changes: 45 additions & 33 deletions src/main/java/frc/robot/Superstructure.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
));
)
);
}
}

Expand Down Expand Up @@ -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()),
Expand Down Expand Up @@ -180,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)
);
}
Expand All @@ -189,52 +204,42 @@ 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;
}


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)
)
);
}
Expand All @@ -248,9 +253,11 @@ public static boolean inTrenchZone() {
&& robotPose.getX() <= zone[1].getX()
&& robotPose.getY() >= zone[0].getY()
&& robotPose.getY() <= zone[1].getY()) {

return true;
}
}

return false;
}

Expand All @@ -262,9 +269,11 @@ public static boolean inTrenchDangerZone() {
&& turretPose.getX() <= zone[1].getX()
&& turretPose.getY() >= zone[0].getY()
&& turretPose.getY() <= zone[1].getY()) {

return true;
}
}

return false;
}

Expand All @@ -280,18 +289,22 @@ public static boolean inBumpZone() {
&& robotPose.getX() <= zone[1].getX()
&& robotPose.getY() >= zone[0].getY()
&& robotPose.getY() <= zone[1].getY()) {

return true;
}
}

return false;
}

// 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)
Expand All @@ -304,7 +317,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;
}
}