Skip to content

Superstructure comments #155

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
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Superstructure comments #155

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
99a6ca2
Merge remote-tracking branch 'origin/main' into superstructure_change…
braelynandthefrogs May 30, 2025
1d83540
more comments, superstructure should be commented now. maybe we shoul…
braelynandthefrogs Jun 6, 2025
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
101 changes: 79 additions & 22 deletions src/main/java/frc/robot/subsystems/SuperStructure.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ private Command colorSet(int r, int g, int b, String name) {
}

private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) {
// repeats scoring sequence if the coral is still in the claw
if (armSensor == null) {
return Commands.sequence(
command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(score));
Expand All @@ -67,15 +68,15 @@ private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score)
}

public Command coralLevelFour(BooleanSupplier score) {
if (branchSensors != null) {
if (branchSensors != null) { // checks if sensor enabled then use for faster scoring
score = branchSensors.withinScoreRange().or(score);
}
return Commands.sequence(
Commands.parallel(
Commands.print("Pre position"),
elevator
.setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS)
.deadlineFor(
.deadlineFor( // keeps spinny claw engaged until coral has been scored
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)),
spinnyClaw.stop())
.withTimeout(0.7),
Expand All @@ -84,20 +85,27 @@ public Command coralLevelFour(BooleanSupplier score) {
Commands.parallel(
elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS),
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_PRE_L4))
.withDeadline(Commands.waitUntil(score)),
.withDeadline(
Commands.waitUntil(
score)), // waits until driver presses the score button or until
// auto scoring happens
armPivot
.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
.withTimeout(1.5)
.until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
score),
Commands.print("Pre preIntake()"),
coralPreIntake().unless(RobotModeTriggers.autonomous()),
coralPreIntake()
.unless(
RobotModeTriggers
.autonomous()), // doesn't go to preintake if auto, should add for otehr
// commands
Commands.print("Post preIntake()"))
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L4").asProxy())
.withName("Coral Level 4");
}

public Command coralLevelThree(BooleanSupplier score) {
public Command coralLevelThree(BooleanSupplier score) { // same as L4
return Commands.sequence(
Commands.parallel(
elevator
Expand All @@ -121,7 +129,7 @@ public Command coralLevelThree(BooleanSupplier score) {
.withName("Coral Level 3");
}

public Command coralLevelTwo(BooleanSupplier score) {
public Command coralLevelTwo(BooleanSupplier score) { // same as L4 and L3
return Commands.sequence(
Commands.parallel(
elevator
Expand Down Expand Up @@ -150,16 +158,19 @@ public Command coralLevelOne(BooleanSupplier score) {
Commands.parallel(
elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS),
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1),
spinnyClaw.stop())
spinnyClaw.stop()) // holds coral without wearing flywheels
.withTimeout(0.5)
.withDeadline(Commands.waitUntil(score)),
spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.25),
spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.25), // spits out coral
Commands.waitSeconds(1), // Wait to clear the reef
coralPreIntake())
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy())
.withName("Coral Level 1");
}

// quickGroundIntake() is used instead since it's faster and still reliable.
// (This one moves the coral intake the normal intake position, then does the normal intake.
// quickGroundIntake() instead hands the coral directly to the claw.)
public Command groundIntake(BooleanSupplier retract) {
if (groundSpinny == null || groundArm == null || intakeSensor == null) {
return Commands.none().withName("ground intake disabled");
Expand All @@ -171,11 +182,19 @@ public Command groundIntake(BooleanSupplier retract) {
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_GROUND_INTAKE),
spinnyClaw.stop(), // just as a backup in case things are silly
groundSpinny.setGroundIntakePower())
.until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)),
.until(
elevator.above(
ElevatorSubsystem
.MIN_EMPTY_GROUND_INTAKE)), // waits until elevator is out of the way
// to lift ground intake
groundArm
.moveToPosition(GroundArm.GROUND_POSITION)
.andThen(groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE))
.withDeadline(Commands.waitUntil(intakeSensor.inIntake().or(retract))),
.withDeadline(
Commands.waitUntil(
intakeSensor
.inIntake()
.or(retract))), // ends ground pickup to stow coral for scoring
groundArm.moveToPosition(GroundArm.STOWED_POSITION),
groundSpinny.setFunnelIntakePower(),
coralPreIntake())
Expand All @@ -184,45 +203,79 @@ public Command groundIntake(BooleanSupplier retract) {
}
}

public Command quickGroundIntake(BooleanSupplier retract) {
// This is the actual version in use. It moves the coral directly into the claw.
public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph
if (groundSpinny == null || groundArm == null || intakeSensor == null) {
// If anything's disabled, completely give up. (Theoretically we might be able to do some
// stuff, but it wasn't worth implementing in the time we had.)
return Commands.none().withName("quick ground intake disabled");
} else {
// BooleanSupplier for whether the claw is full. This is a separate variable so that it gets a
// name (so it's easier to read).
BooleanSupplier clawFull = armSensor != null ? armSensor.inClaw() : () -> false;
// Make the big sequence.
return Commands.sequence(
// Initial setup- Move elevator high enough for ground arm to be clear, start moving
// arm pivot, stop the spinny claw, and start spinning the ground intake
Commands.parallel(
elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE),
armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE),
spinnyClaw.stop(), // just as a backup in case things are silly
groundSpinny.setGroundIntakePower())
// Move on even if arm isn't in position yet as long as elevator is high enough
.until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)),
// Core intake sequence
Commands.sequence(
// Deploy the ground arm (and wait until it reaches the position).
groundArm.moveToPosition(GroundArm.GROUND_POSITION),
// After it's deployed, apply a constant voltage to press it into the bumper
// and continue.
groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE),
// Grabbing segment
Commands.parallel(
// These three are the initial setup: Move elevator down to the handoff
// height, make sure armPivot finishes moving to the right height, and
// spin claw
elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE),
armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE),
spinnyClaw.coralIntakePower(),
// Run the intake with occassional jitters. (This is stopped when the core
// intake sequence is stopped.)
Commands.sequence(
// Wait until the jitter condition (stalling for at least 0.25
// seconds and we don't have a coral)
Commands.waitUntil(
groundSpinny
.stalling()
.debounce(0.25)
.and(intakeSensor.inIntake().negate())),
// Then, spin out at the jitter speed for 0.1 seconds.
groundSpinny.holdGroundIntakeJitterSpeed().withTimeout(0.1))
// Whenever we move on, make sure to set the ground spinny back to the
// ground intake speed. The finallyDo() ensures this happens no matter
// when we move on.
.finallyDo(() -> groundSpinny.imperativeSetGroundIntakePower())
// Repeat the wait and jitter until we move on.
.repeatedly()))
// Move on from the core intake sequence as soon as we either have something in
// ground intake or we tell it to retract.
.withDeadline(Commands.waitUntil(intakeSensor.inIntake().or(retract))),
// Retract the groundArm all the way to stowed (so that we aren't slowing down to stop
// in the middle), but move on to the next command when we're at the handoff point.
groundArm
.moveToPosition(GroundArm.STOWED_POSITION)
.until(groundArm.atPosition(GroundArm.QUICK_INTAKE_POSITION)),
// Spin groundSpinny out, but skip if we lost the coral.
groundSpinny.setQuickHandoffExtakeSpeed().onlyIf(armSensor.inClaw()),
// Go back to stow, but skip if we lost the coral.
coralStow().onlyIf(armSensor.inClaw()),
// If we lost the coral, reset for a quick retry.
Commands.parallel(
elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE),
armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE),
spinnyClaw.stop())
.onlyIf(armSensor.inClaw().negate()))
// Don't run the entire sequence at all if the claw is full.
.unless(clawFull)
.withName("Ground Intake");
}
Expand All @@ -233,15 +286,15 @@ public Command coralStow() {
() -> {
BranchHeight currentBranchHeight = branchHeight.get();
double elevatorHeight =
switch (currentBranchHeight) {
switch (currentBranchHeight) { // used to stow closer to the scoring height
case CORAL_LEVEL_FOUR -> ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS;
case CORAL_LEVEL_THREE -> ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS;
case CORAL_LEVEL_TWO -> ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS;
case CORAL_LEVEL_ONE -> ElevatorSubsystem.CORAL_LEVEL_ONE_POS;
};
return Commands.parallel(
elevator.setLevel(elevatorHeight),
Commands.sequence(
Commands.sequence( // waits to raise the arm until elevator is above preintake
Commands.waitUntil(elevator.above(ElevatorSubsystem.CORAL_PRE_INTAKE)),
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP)),
spinnyClaw.stop());
Expand All @@ -254,19 +307,21 @@ public Command coralPreIntake() {
return Commands.parallel(
elevator.setLevel(ElevatorSubsystem.CORAL_PRE_INTAKE),
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_DOWN),
spinnyClaw.coralRejectPower())
spinnyClaw.coralRejectPower()) // keeps coral out while waiting to intake
.andThen(Commands.print("end of preIntake()"))
.withName("PreIntake");
}

public Trigger inCoralPreIntakePosition() {
public Trigger
inCoralPreIntakePosition() { // creates a trigger for coral intake that waits until robot is
// in preintake position
return new Trigger(
() ->
elevator.atPosition(ElevatorSubsystem.CORAL_PRE_INTAKE)
&& armPivot.atPosition(ArmPivot.CORAL_PRESET_DOWN));
}

public Command coralIntake() {
public Command coralIntake() { // yummy coral
return Commands.sequence(
Commands.sequence(
Commands.parallel(
Expand All @@ -277,7 +332,7 @@ public Command coralIntake() {
.withName("Coral Intake");
}

public Command algaeLevelThreeFourIntake() {
public Command algaeLevelThreeFourIntake() { // removes algae from upper reef
return Commands.sequence(
Commands.parallel(
spinnyClaw.algaeIntakePower(),
Expand All @@ -286,7 +341,7 @@ public Command algaeLevelThreeFourIntake() {
.withName("Algae L3-L4 Intake");
}

public Command algaeLevelTwoThreeIntake() {
public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef
return Commands.sequence(
Commands.parallel(
spinnyClaw.algaeIntakePower(),
Expand All @@ -295,7 +350,8 @@ public Command algaeLevelTwoThreeIntake() {
.withName("Algae L2-L3 Intake");
}

public Command algaeGroundIntake() {
public Command
algaeGroundIntake() { // picks up algae on the ground (mostly only works against a wall)
return Commands.parallel(
spinnyClaw.algaeIntakePower(),
Commands.sequence(
Expand All @@ -304,7 +360,7 @@ public Command algaeGroundIntake() {
.withName("Algae Ground Intake");
}

public Command algaeStow() {
public Command algaeStow() { // holds algae
return Commands.parallel(
elevator.setLevel(ElevatorSubsystem.ALGAE_STOWED),
armPivot.moveToPosition(ArmPivot.ALGAE_STOWED),
Expand All @@ -313,7 +369,7 @@ public Command algaeStow() {
.withName("Algae Stow");
}

public Command algaeProcessorScore(BooleanSupplier score) {
public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in processor
return Commands.sequence(
Commands.parallel(
spinnyClaw.algaeGripIntakePower(),
Expand All @@ -334,7 +390,8 @@ public Command algaeNetScore(BooleanSupplier score) {
Commands.waitUntil(score),
spinnyClaw.algaeHoldExtakePower().withTimeout(0.7),
Commands.waitSeconds(0.7),
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP),
armPivot.moveToPosition(
ArmPivot.CORAL_PRESET_UP), // added to prevent hitting the barge after scoring net
elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR))
.withName("Algae Net Score");
}
Expand Down