Skip to content

move superstructure outside of the subsystem folder #147

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

Draft
braelynandthefrogs wants to merge 6 commits into
base: main
Choose a base branch
from
Draft

move superstructure outside of the subsystem folder #147

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
6ef42db
moved superstructure outside of the subsystem folder
braelynandthefrogs Apr 1, 2025
0eb3693
Merge remote-tracking branch 'origin/main' into superstructure_change…
braelynandthefrogs Apr 15, 2025
1c07552
Fixing merge conflict
markpete Nov 29, 2025
8edd60f
Post merge clean-up and removal of dead code
markpete Nov 29, 2025
4a1bfa4
Merge branch 'main' of https://github.com/robototes/Reefscape2025 int…
markpete Nov 29, 2025
acb87ba
Fixing Spotless issue
markpete Nov 29, 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
33 changes: 15 additions & 18 deletions src/main/java/frc/robot/Controls.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@
import frc.robot.subsystems.ArmPivot;
import frc.robot.subsystems.ElevatorSubsystem;
import frc.robot.subsystems.GroundArm;
import frc.robot.subsystems.SuperStructure;
import frc.robot.subsystems.auto.AutoAlgaeHeights;
import frc.robot.subsystems.auto.AutoAlign;
import frc.robot.subsystems.auto.BargeAlign;
Expand Down Expand Up @@ -72,8 +71,6 @@ public class Controls {
RobotType.getCurrent() == RobotType.BONK
? BonkTunerConstants.kSpeedAt12Volts.in(MetersPerSecond)
: CompTunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private final double MAX_ACCELERATION = 50;
private final double MAX_ROTATION_ACCELERATION = 50;
// kSpeedAt12Volts desired top speed
public static double MaxAngularRate =
RotationsPerSecond.of(0.75)
Expand Down Expand Up @@ -456,41 +453,41 @@ private Command getCoralBranchHeightCommand(ScoringType version) {
return switch (branchHeight) {
case CORAL_LEVEL_FOUR -> superStructure
.coralLevelFour(soloController.rightBumper())
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
.andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE);
case CORAL_LEVEL_THREE -> superStructure
.coralLevelThree(
soloController.rightBumper(),
() -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB))
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
.andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE);
case CORAL_LEVEL_TWO -> superStructure
.coralLevelTwo(
soloController.rightBumper(),
() -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB))
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
.andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE);
case CORAL_LEVEL_ONE -> superStructure
.coralLevelOne(
soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1LB))
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
.andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE);
};
} else if (version == ScoringType.SOLOC_RIGHT) {
return switch (branchHeight) {
case CORAL_LEVEL_FOUR -> superStructure
.coralLevelFour(soloController.rightBumper())
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
.andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE);
case CORAL_LEVEL_THREE -> superStructure
.coralLevelThree(
soloController.rightBumper(),
() -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB))
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
.andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE);
case CORAL_LEVEL_TWO -> superStructure
.coralLevelTwo(
soloController.rightBumper(),
() -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB))
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
.andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE);
case CORAL_LEVEL_ONE -> superStructure
.coralLevelOne(
soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1RB))
.andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE);
.andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE);
};
} else {
return switch (branchHeight) {
Expand Down Expand Up @@ -722,7 +719,7 @@ private void configureSpinnyClawBindings() {
if (s.spinnyClawSubsytem == null) {
return;
}
s.spinnyClawSubsytem.setScoringMode(() -> scoringMode);

// Claw controls bindings go here
connected(armPivotSpinnyClawController)
.and(armPivotSpinnyClawController.leftBumper())
Expand Down Expand Up @@ -976,7 +973,7 @@ private void configureSoloControllerBindings() {
Commands.sequence(
bargeScoreCommand,
Commands.runOnce(
() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE));
() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE));
}
case NO_GAME_PIECE -> {
scoreCommand =
Expand All @@ -994,12 +991,12 @@ private void configureSoloControllerBindings() {
}));
soloController
.leftTrigger()
.and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
.and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW)
.and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB));
soloController
.leftTrigger()
.and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
.and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW)
.and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE)
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1LB));
Comment on lines 992 to 1001
Copy link

@gemini-code-assist gemini-code-assist bot Nov 30, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

This block of code for the leftTrigger is very similar to the block for the rightTrigger (lines 1035-1044). The only differences are the trigger itself (leftTrigger vs rightTrigger) and the AlignType enums used (LEFTB/L1LB vs RIGHTB/L1RB).

To improve maintainability and reduce code duplication, consider extracting this logic into a private helper method. This method could take the trigger and the alignment types as parameters.

For example:

private void configureAutoAlignForTrigger(Trigger trigger, AutoAlign.AlignType alignTypeB, AutoAlign.AlignType alignTypeL1) {
    trigger
        .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW)
        .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
        .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, alignTypeB));
    trigger
        .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW)
        .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE)
        .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, alignTypeL1));
}

// Then in configureSoloControllerBindings():
configureAutoAlignForTrigger(soloController.leftTrigger(), AutoAlign.AlignType.LEFTB, AutoAlign.AlignType.L1LB);
configureAutoAlignForTrigger(soloController.rightTrigger(), AutoAlign.AlignType.RIGHTB, AutoAlign.AlignType.L1RB);

// Processor + Auto align right + Select scoring mode Coral
Expand All @@ -1023,7 +1020,7 @@ private void configureSoloControllerBindings() {
soloController.rightBumper()),
Commands.waitSeconds(0.7),
Commands.runOnce(
() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE))
() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE))
.withName("Processor score");
case NO_GAME_PIECE -> Commands.parallel(
Commands.runOnce(() -> intakeMode = ScoringMode.CORAL)
Expand All @@ -1037,12 +1034,12 @@ private void configureSoloControllerBindings() {
.withName("score"));
soloController
.rightTrigger()
.and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
.and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW)
.and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE)
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB));
soloController
.rightTrigger()
.and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW)
.and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW)
.and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE)
.whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1RB));
// Ground Intake
Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.Subsystems.SubsystemConstants;
import frc.robot.subsystems.SuperStructure;
import frc.robot.subsystems.auto.AutoBuilderConfig;
import frc.robot.subsystems.auto.AutoLogic;
import frc.robot.subsystems.auto.AutonomousField;
Expand Down
7 changes: 6 additions & 1 deletion .../frc/robot/subsystems/SuperStructure.java → src/main/java/frc/robot/SuperStructure.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
package frc.robot.subsystems;
package frc.robot;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
Expand All @@ -8,6 +8,11 @@
import frc.robot.sensors.BranchSensors;
import frc.robot.sensors.ElevatorLight;
import frc.robot.sensors.IntakeSensor;
import frc.robot.subsystems.ArmPivot;
import frc.robot.subsystems.ElevatorSubsystem;
import frc.robot.subsystems.GroundArm;
import frc.robot.subsystems.GroundSpinny;
import frc.robot.subsystems.SpinnyClaw;
import frc.robot.util.BranchHeight;
import java.util.Set;
import java.util.function.BooleanSupplier;
Expand Down
6 changes: 0 additions & 6 deletions src/main/java/frc/robot/subsystems/ClimbPivot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,6 @@ public enum TargetPositions {
private final double FORWARD_SOFT_STOP = -0.07;
private final double REVERSE_SOFT_STOP = -78;
private final double CLIMB_OUT_SPEED = 1.0;
private final double CLIMB_HOLD_STOWED = -0.001;
private final double CLIMB_HOLD_CLIMBOUT = -0.0;
private final double CLIMB_HOLD_CLIMBED = -0.0705;
private final double CLIMB_IN_SPEED = -0.75;

private final double climbInKp = 50;
Expand All @@ -74,7 +71,6 @@ public enum TargetPositions {
private TargetPositions selectedPos = TargetPositions.STOWED;
private double maxTargetPos = STOWED_MAX_PRESET;
private double minTargetPos = STOWED_MIN_PRESET;
private double holdSpeed = CLIMB_HOLD_STOWED;

// if moveComplete is true it wont move regardless of if its in range. This is to ensure that
// when disabled, when re-enabled it doesnt start moving.
Expand Down Expand Up @@ -190,7 +186,6 @@ private void setTargetPos(TargetPositions newTargetPos) {
selectedPos = TargetPositions.CLIMB_OUT;
maxTargetPos = CLIMB_OUT_MAX_PRESET;
minTargetPos = CLIMB_OUT_MIN_PRESET;
holdSpeed = CLIMB_HOLD_STOWED;
moveComplete = false;
}
case CLIMBED -> {
Expand All @@ -199,7 +194,6 @@ private void setTargetPos(TargetPositions newTargetPos) {
selectedPos = TargetPositions.CLIMBED;
maxTargetPos = CLIMBED_MAX_PRESET;
minTargetPos = CLIMBED_MIN_PRESET;
holdSpeed = CLIMB_HOLD_CLIMBOUT;
moveComplete = false;
}
}
Expand Down
3 changes: 0 additions & 3 deletions src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,11 +60,8 @@ public class ElevatorSubsystem extends SubsystemBase {
private final double ELEVATOR_KS = 0.33878;
private final double ELEVATOR_KV = 0.12975;
private final double ELEVATOR_KA = 0.0070325;
private final double REVERSE_SOFT_LIMIT = -0.05;
private final double FORWARD_SOFT_LIMIT = 38;
public static final double UP_VOLTAGE = 5;
private final double DOWN_VOLTAGE = -3;
private final double HOLD_VOLTAGE = 0.6;
// create a Motion Magic request, voltage output
private final MotionMagicVoltage m_request = new MotionMagicVoltage(0);

Expand Down
6 changes: 0 additions & 6 deletions src/main/java/frc/robot/subsystems/SpinnyClaw.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Hardware;
import frc.robot.sensors.ArmSensor;
import frc.robot.util.ScoringMode;
import java.util.function.Supplier;

public class SpinnyClaw extends SubsystemBase {
Expand All @@ -37,7 +36,6 @@ public class SpinnyClaw extends SubsystemBase {
private final TalonFX motor;
// ArmSensor
private final ArmSensor armSensor;
private Supplier<ScoringMode> scoringMode = () -> ScoringMode.CORAL;

// alerts
private final Alert NotConnectedError =
Expand All @@ -52,10 +50,6 @@ public SpinnyClaw(ArmSensor armSensor) {
logTabs();
}

public void setScoringMode(Supplier<ScoringMode> scoringMode) {
this.scoringMode = scoringMode;
}

// (+) is to intake out, and (-) is in
public Command movingVoltage(Supplier<Voltage> speedControl) {
return run(() -> motor.setVoltage(speedControl.get().in(Volts)))
Expand Down
17 changes: 0 additions & 17 deletions src/main/java/frc/robot/subsystems/VisionSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
import edu.wpi.first.networktables.GenericSubscriber;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
Expand All @@ -25,7 +24,6 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Hardware;
import java.util.Optional;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
Expand Down Expand Up @@ -278,19 +276,4 @@ public double getTimeSinceLastReading() {
public double getDistanceToTarget() {
return (double) Math.round(Distance * 1000) / 1000;
}

// configured for 2025 reefscape to filter out any tag besides the reef tags depending on allaince
private static boolean BadAprilTagDetector(PhotonPipelineResult r) {
boolean isRed = DriverStation.getAlliance().equals(Optional.of(DriverStation.Alliance.Red));
boolean isBlue = DriverStation.getAlliance().equals(Optional.of(DriverStation.Alliance.Blue));
for (var t : r.getTargets()) {
boolean isRedReef = 6 <= t.getFiducialId() && t.getFiducialId() <= 11;
boolean isBlueReef = 17 <= t.getFiducialId() && t.getFiducialId() <= 22;
boolean isValid = isBlueReef && !isRed || isRedReef && !isBlue;
if (!isValid) {
return true;
}
}
return false;
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Controls;
import frc.robot.subsystems.SuperStructure;
import frc.robot.SuperStructure;
import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
import frc.robot.util.AllianceUtils;
import frc.robot.util.ScoringMode;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/auto/BargeAlign.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.subsystems.SuperStructure;
import frc.robot.SuperStructure;
import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
Expand Down
41 changes: 0 additions & 41 deletions src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,10 +41,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/* Swerve requests to apply during SysId characterization */
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization =
new SwerveRequest.SysIdSwerveTranslation();
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization =
new SwerveRequest.SysIdSwerveSteerGains();
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization =
new SwerveRequest.SysIdSwerveRotation();

/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
private final SysIdRoutine m_sysIdRoutineTranslation =
Expand All @@ -58,43 +54,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
new SysIdRoutine.Mechanism(
output -> setControl(m_translationCharacterization.withVolts(output)), null, this));

/* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
private final SysIdRoutine m_sysIdRoutineSteer =
new SysIdRoutine(
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(7), // Use dynamic voltage of 7 V
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdSteer_State", state.toString())),
new SysIdRoutine.Mechanism(
volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this));

/*
* SysId routine for characterizing rotation.
* This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
*/
private final SysIdRoutine m_sysIdRoutineRotation =
new SysIdRoutine(
new SysIdRoutine.Config(
/* This is in radians per second², but SysId only supports "volts per second" */
Volts.of(Math.PI / 6).per(Second),
/* This is in radians per second, but SysId only supports "volts" */
Volts.of(Math.PI),
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdRotation_State", state.toString())),
new SysIdRoutine.Mechanism(
output -> {
/* output is actually radians per second, but SysId only supports "volts" */
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
/* also log the requested output for SysId */
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
},
null,
this));

/* The SysId routine to test */
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;

Expand Down