From 687b6427bc989a281bde81f6b307df6ae1d1cc71 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 22 Mar 2025 11:15:36 -0700 Subject: [PATCH 01/11] Experiment with having interfaces for subsystems --- .../java/com/team2813/RobotContainer.java | 78 +++++++------------ .../elevator/DefaultCommand.java} | 9 +-- .../subsystems/elevator/Elevator.java | 67 ++++++++++++++++ .../ElevatorSubsystem.java} | 73 +++++++++++------ 4 files changed, 146 insertions(+), 81 deletions(-) rename src/main/java/com/team2813/{commands/ElevatorDefaultCommand.java => subsystems/elevator/DefaultCommand.java} (75%) create mode 100644 src/main/java/com/team2813/subsystems/elevator/Elevator.java rename src/main/java/com/team2813/subsystems/{Elevator.java => elevator/ElevatorSubsystem.java} (58%) diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index b8e1cb1d..df730a31 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -14,10 +14,14 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.events.EventTrigger; -import com.team2813.commands.*; +import com.team2813.commands.LockFunctionCommand; +import com.team2813.commands.ManuelIntakePivot; +import com.team2813.commands.OuttakeCommand; +import com.team2813.commands.RobotLocalization; import com.team2813.lib2813.limelight.BotPoseEstimate; import com.team2813.lib2813.limelight.Limelight; import com.team2813.subsystems.*; +import com.team2813.subsystems.elevator.Elevator; import com.team2813.sysid.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -53,7 +57,7 @@ public class RobotContainer implements AutoCloseable { public RobotContainer(ShuffleboardTabs shuffleboard, NetworkTableInstance networkTableInstance) { var localization = new RobotLocalization(networkTableInstance); this.drive = new Drive(networkTableInstance, localization); - this.elevator = new Elevator(networkTableInstance); + this.elevator = Elevator.create(() -> -OPERATOR_CONTROLLER.getRightY()); this.intakePivot = new IntakePivot(networkTableInstance); this.climb = new Climb(networkTableInstance); this.intake = new Intake(networkTableInstance); @@ -89,14 +93,14 @@ private static void configureAutoCommands( new ParallelCommandGroup( new InstantCommand( () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), - new InstantCommand(() -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator))); + elevator.setSetpointCommand(Elevator.Position.BOTTOM))); NamedCommands.registerCommand( "PrepareL3", new ParallelCommandGroup( new InstantCommand( () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), - new InstantCommand(() -> elevator.setSetpoint(Elevator.Position.TOP), elevator))); + elevator.setSetpointCommand(Elevator.Position.TOP))); NamedCommands.registerCommand( "PrepareScore", @@ -107,11 +111,7 @@ private static void configureAutoCommands( "ScoreL2", new SequentialCommandGroup( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), - elevator) - .withTimeout(SECONDS_2), + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), new LockFunctionCommand( intakePivot::atPosition, () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), @@ -121,7 +121,7 @@ private static void configureAutoCommands( new WaitCommand(DROP_CORAL), new ParallelCommandGroup( intake.stopMotorCommand(), - new InstantCommand(elevator::disable, elevator), + elevator.disableCommand(), new InstantCommand( () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); @@ -136,11 +136,7 @@ private static void configureAutoCommands( NamedCommands.registerCommand( "ScoreL3", new SequentialCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.TOP), - elevator) - .withTimeout(SECONDS_2), + elevator.moveToPositionCommand(Elevator.Position.TOP), new LockFunctionCommand( intakePivot::atPosition, () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), @@ -150,7 +146,7 @@ private static void configureAutoCommands( new WaitCommand(DROP_CORAL), new ParallelCommandGroup( intake.stopMotorCommand(), - new InstantCommand(() -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator), + elevator.setSetpointCommand(Elevator.Position.BOTTOM), new InstantCommand( () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); @@ -158,11 +154,7 @@ private static void configureAutoCommands( "BumpAlgaeLow", new SequentialCommandGroup( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), - elevator) - .withTimeout(SECONDS_2), + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), new LockFunctionCommand( intakePivot::atPosition, () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), @@ -172,7 +164,7 @@ private static void configureAutoCommands( new WaitCommand(SECONDS_1), // TODO: Wait until we bump low algae new ParallelCommandGroup( intake.stopMotorCommand(), - new InstantCommand(() -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator), + elevator.setSetpointCommand(Elevator.Position.BOTTOM), new InstantCommand( () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); @@ -181,12 +173,7 @@ private static void configureAutoCommands( new SequentialCommandGroup( new ParallelCommandGroup( new SequentialCommandGroup( - new WaitCommand(0.05), - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.TOP), - elevator) - .withTimeout(SECONDS_2)), + new WaitCommand(0.05), elevator.setSetpointCommand(Elevator.Position.TOP)), new LockFunctionCommand( intakePivot::atPosition, () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), @@ -196,7 +183,7 @@ private static void configureAutoCommands( new WaitCommand(SECONDS_1), // TODO: Wait until we bump high algae new ParallelCommandGroup( intake.stopMotorCommand(), - new InstantCommand(() -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator), + elevator.setSetpointCommand(Elevator.Position.BOTTOM), new InstantCommand( () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); @@ -204,11 +191,7 @@ private static void configureAutoCommands( "IntakeCoral", new SequentialCommandGroup( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), - elevator) - .withTimeout(SECONDS_2), + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), new LockFunctionCommand( intakePivot::atPosition, () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), @@ -218,7 +201,7 @@ private static void configureAutoCommands( new WaitUntilCommand(intake::hasCoral).withTimeout(INTAKE_TIME), new ParallelCommandGroup( intake.intakeItemCommand(), - new InstantCommand(elevator::disable, elevator), + elevator.disableCommand(), new InstantCommand(intakePivot::disable, intakePivot)))); new EventTrigger("PrepareL2") @@ -226,12 +209,13 @@ private static void configureAutoCommands( new ParallelCommandGroup( new InstantCommand( () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), - new InstantCommand( - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator))); + elevator.setSetpointCommand(Elevator.Position.BOTTOM))); new EventTrigger("PrepareL3") .onTrue( - new DeferredCommand( - () -> NamedCommands.getCommand("PrepareL3"), Set.of(intakePivot, elevator))); + new ParallelCommandGroup( + new InstantCommand( + () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), + elevator.setSetpointCommand(Elevator.Position.TOP))); } private static SendableChooser configureAuto( @@ -368,11 +352,7 @@ private void configureBindings(RobotLocalization localization) { INTAKE_BUTTON.whileTrue( new SequentialCommandGroup( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), - elevator) - .withTimeout(Units.Seconds.of(2)), + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), new LockFunctionCommand( intakePivot::atPosition, () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), @@ -411,18 +391,14 @@ private void configureBindings(RobotLocalization localization) { PREP_L2_CORAL.onTrue( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), - elevator), + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), new LockFunctionCommand( intakePivot::atPosition, () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot))); PREP_L3_CORAL.onTrue( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, () -> elevator.setSetpoint(Elevator.Position.TOP), elevator), + elevator.moveToPositionCommand(Elevator.Position.TOP), new LockFunctionCommand( intakePivot::atPosition, () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), @@ -431,8 +407,6 @@ private void configureBindings(RobotLocalization localization) { R2.whileTrue( new InstantCommand() );*/ - elevator.setDefaultCommand( - new ElevatorDefaultCommand(elevator, () -> -OPERATOR_CONTROLLER.getRightY())); intakePivot.setDefaultCommand( new ManuelIntakePivot(intakePivot, () -> -OPERATOR_CONTROLLER.getLeftY())); diff --git a/src/main/java/com/team2813/commands/ElevatorDefaultCommand.java b/src/main/java/com/team2813/subsystems/elevator/DefaultCommand.java similarity index 75% rename from src/main/java/com/team2813/commands/ElevatorDefaultCommand.java rename to src/main/java/com/team2813/subsystems/elevator/DefaultCommand.java index 19f6a980..9f98e63e 100644 --- a/src/main/java/com/team2813/commands/ElevatorDefaultCommand.java +++ b/src/main/java/com/team2813/subsystems/elevator/DefaultCommand.java @@ -1,15 +1,14 @@ -package com.team2813.commands; +package com.team2813.subsystems.elevator; import com.team2813.lib2813.control.ControlMode; -import com.team2813.subsystems.Elevator; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.DoubleSupplier; -public class ElevatorDefaultCommand extends Command { - private final Elevator elevator; +class DefaultCommand extends Command { + private final ElevatorSubsystem elevator; private final DoubleSupplier movement; - public ElevatorDefaultCommand(Elevator elevator, DoubleSupplier movement) { + DefaultCommand(ElevatorSubsystem elevator, DoubleSupplier movement) { this.elevator = elevator; this.movement = movement; addRequirements(elevator); diff --git a/src/main/java/com/team2813/subsystems/elevator/Elevator.java b/src/main/java/com/team2813/subsystems/elevator/Elevator.java new file mode 100644 index 00000000..90656476 --- /dev/null +++ b/src/main/java/com/team2813/subsystems/elevator/Elevator.java @@ -0,0 +1,67 @@ +package com.team2813.subsystems.elevator; + +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public interface Elevator { + + /** + * Creates an instance. + * + * @param movement Controller input. + */ + static Elevator create(DoubleSupplier movement) { + return new ElevatorSubsystem(NetworkTableInstance.getDefault(), movement); + } + + /** + * Returns a command that sets the setpoint and waits for the elevator to reach that point. + * + * @param position The desired position. + */ + Command moveToPositionCommand(Position position); + + /** + * Returns a command that sets the setpoint to the given value. + * + *

The returned command will complete immediately after executing. The default elevator command + * will move the elevator to the given setpoint. + * + * @param position The desired position. + */ + Command setSetpointCommand(Position position); + + /** + * Returns a command that waits for the elevator to reach the current setpoint. + * + *

If the elevator does not reach the setpoint in a reasonable period of time, the returned + * command will cancel itself. + */ + Command waitForSetpointCommand(); + + Command disableCommand(); + + boolean atPosition(); + + public enum Position implements Supplier { + BOTTOM(-0.212500), + TEST(10), + TOP(16.358496); + + private final Angle position; + + Position(double position) { + this.position = Rotations.of(position); + } + + @Override + public Angle get() { + return position; + } + } +} diff --git a/src/main/java/com/team2813/subsystems/Elevator.java b/src/main/java/com/team2813/subsystems/elevator/ElevatorSubsystem.java similarity index 58% rename from src/main/java/com/team2813/subsystems/Elevator.java rename to src/main/java/com/team2813/subsystems/elevator/ElevatorSubsystem.java index c410f48e..de9582b2 100644 --- a/src/main/java/com/team2813/subsystems/Elevator.java +++ b/src/main/java/com/team2813/subsystems/elevator/ElevatorSubsystem.java @@ -1,10 +1,11 @@ -package com.team2813.subsystems; +package com.team2813.subsystems.elevator; import static com.team2813.Constants.ELEVATOR_1; import static com.team2813.Constants.ELEVATOR_2; -import static edu.wpi.first.units.Units.Rotations; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.team2813.commands.LockFunctionCommand; import com.team2813.lib2813.control.ControlMode; import com.team2813.lib2813.control.InvertType; import com.team2813.lib2813.control.motors.TalonFXWrapper; @@ -15,12 +16,18 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; -import java.util.function.Supplier; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj2.command.*; +import java.util.Set; +import java.util.function.DoubleSupplier; /** This is the Elevator. His name is Pablo. Please be kind to him and say hi. Have a nice day! */ -public class Elevator extends MotorSubsystem { +class ElevatorSubsystem extends MotorSubsystem implements Elevator { + private static final Time MOVEMENT_TIMEOUT = Units.Seconds.of(2); + private final BooleanPublisher atPosition; + private final DoublePublisher position; + private double lastSetpointSetTime; // You see a message written in blood on the wall... // It reads: "THIS CODE WILL LIKELY HAVE TO BE *MAJORLY* REFACTORED @@ -28,7 +35,7 @@ public class Elevator extends MotorSubsystem { // HERE BE DRAGONS. // Your companion notes: "...jeez... that is a lot of blood... couldn't they just leave a paper // taped to the wall, rather than raid a blood donation clinic." - public Elevator(NetworkTableInstance networkTableInstance) { + ElevatorSubsystem(NetworkTableInstance networkTableInstance, DoubleSupplier movement) { super( new MotorSubsystemConfiguration(getMotor()) .controlMode(ControlMode.VOLTAGE) @@ -38,6 +45,8 @@ public Elevator(NetworkTableInstance networkTableInstance) { NetworkTable networkTable = networkTableInstance.getTable("Elevator"); atPosition = networkTable.getBooleanTopic("at position").publish(); position = networkTable.getDoubleTopic("position").publish(); + + setDefaultCommand(new DefaultCommand(this, movement)); } private static TalonFXWrapper getMotor() { @@ -48,34 +57,50 @@ private static TalonFXWrapper getMotor() { } @Override - protected void useOutput(double output, double setpoint) { - super.useOutput(MathUtil.clamp(output, -6, 6), setpoint); + public Command moveToPositionCommand(Position position) { + return new LockFunctionCommand(this::atPosition, () -> this.setSetpoint(position), this) + .withTimeout(MOVEMENT_TIMEOUT); } @Override - public Current getAppliedCurrent() { - return motor.getAppliedCurrent(); + public Command waitForSetpointCommand() { + return new DeferredCommand( + () -> { + if (atPosition()) { + return Commands.none(); + } + double elapsedSecs = Utils.getCurrentTimeSeconds() - lastSetpointSetTime; + Time timeout = MOVEMENT_TIMEOUT.minus(Units.Seconds.of(elapsedSecs)); + return Commands.waitUntil(this::atPosition).withTimeout(timeout); + }, + Set.of(this)); } - public enum Position implements Supplier { - BOTTOM(-0.212500), - TEST(10), - TOP(16.358496); + @Override + public Command setSetpointCommand(Position position) { + return new InstantCommand(() -> setSetpoint(position), this); + } - private final Angle position; + @Override + public void setSetpoint(Position position) { + super.setSetpoint(position); + lastSetpointSetTime = Utils.getCurrentTimeSeconds(); + } - Position(double position) { - this.position = Rotations.of(position); - } + @Override + public Command disableCommand() { + return new InstantCommand(this::disable, this); + } - @Override - public Angle get() { - return position; - } + @Override + protected void useOutput(double output, double setpoint) { + super.useOutput(MathUtil.clamp(output, -6, 6), setpoint); } - private final BooleanPublisher atPosition; - private final DoublePublisher position; + @Override + public Current getAppliedCurrent() { + return motor.getAppliedCurrent(); + } @Override public void periodic() { From 3eb36492ec3f55bb234fcaa52f5560b80098aaea Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Wed, 26 Mar 2025 10:06:30 -0700 Subject: [PATCH 02/11] Make Intake interface --- .../java/com/team2813/RobotContainer.java | 3 +- .../com/team2813/commands/OuttakeCommand.java | 2 +- .../ParameterizedIntakeSubsystem.java | 2 +- .../team2813/subsystems/intake/Intake.java | 35 +++++ .../IntakeSubsystem.java} | 31 +++- .../com/team2813/subsystems/IntakeTest.java | 143 ------------------ .../intake/IntakeSubsystemTest.java | 105 +++++++++++++ 7 files changed, 171 insertions(+), 150 deletions(-) create mode 100644 src/main/java/com/team2813/subsystems/intake/Intake.java rename src/main/java/com/team2813/subsystems/{Intake.java => intake/IntakeSubsystem.java} (76%) delete mode 100644 src/test/java/com/team2813/subsystems/IntakeTest.java create mode 100644 src/test/java/com/team2813/subsystems/intake/IntakeSubsystemTest.java diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index df730a31..8d5ee0b3 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -22,6 +22,7 @@ import com.team2813.lib2813.limelight.Limelight; import com.team2813.subsystems.*; import com.team2813.subsystems.elevator.Elevator; +import com.team2813.subsystems.intake.Intake; import com.team2813.sysid.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -60,7 +61,7 @@ public RobotContainer(ShuffleboardTabs shuffleboard, NetworkTableInstance networ this.elevator = Elevator.create(() -> -OPERATOR_CONTROLLER.getRightY()); this.intakePivot = new IntakePivot(networkTableInstance); this.climb = new Climb(networkTableInstance); - this.intake = new Intake(networkTableInstance); + this.intake = Intake.create(networkTableInstance); this.groundIntakePivot = new GroundIntakePivot(networkTableInstance); autoChooser = configureAuto(drive, elevator, intakePivot, intake, groundIntake, groundIntakePivot); diff --git a/src/main/java/com/team2813/commands/OuttakeCommand.java b/src/main/java/com/team2813/commands/OuttakeCommand.java index b794404b..2ae09211 100644 --- a/src/main/java/com/team2813/commands/OuttakeCommand.java +++ b/src/main/java/com/team2813/commands/OuttakeCommand.java @@ -2,7 +2,7 @@ import com.team2813.subsystems.GroundIntake; import com.team2813.subsystems.GroundIntakePivot; -import com.team2813.subsystems.Intake; +import com.team2813.subsystems.intake.Intake; import edu.wpi.first.wpilibj2.command.*; public final class OuttakeCommand { diff --git a/src/main/java/com/team2813/subsystems/ParameterizedIntakeSubsystem.java b/src/main/java/com/team2813/subsystems/ParameterizedIntakeSubsystem.java index 0a45153b..0e37a538 100644 --- a/src/main/java/com/team2813/subsystems/ParameterizedIntakeSubsystem.java +++ b/src/main/java/com/team2813/subsystems/ParameterizedIntakeSubsystem.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -abstract class ParameterizedIntakeSubsystem extends SubsystemBase implements AutoCloseable { +public abstract class ParameterizedIntakeSubsystem extends SubsystemBase implements AutoCloseable { private final PIDMotor intakeMotor; private final Params params; diff --git a/src/main/java/com/team2813/subsystems/intake/Intake.java b/src/main/java/com/team2813/subsystems/intake/Intake.java new file mode 100644 index 00000000..67243c96 --- /dev/null +++ b/src/main/java/com/team2813/subsystems/intake/Intake.java @@ -0,0 +1,35 @@ +package com.team2813.subsystems.intake; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public interface Intake extends AutoCloseable { + + static Intake create() { + return create(NetworkTableInstance.getDefault()); + } + + static Intake create(NetworkTableInstance ntInstance) { + return new IntakeSubsystem(ntInstance); + } + + boolean hasCoral(); + + Command bumpAlgaeCommand(); + + Command intakeItemCommand(); + + Command outtakeItemCommand(); + + Command slowOuttakeItemCommand(); + + Command stopMotorCommand(); + + Subsystem asSubsystem(); + + void stopMotor(); + + @Override + void close(); +} diff --git a/src/main/java/com/team2813/subsystems/Intake.java b/src/main/java/com/team2813/subsystems/intake/IntakeSubsystem.java similarity index 76% rename from src/main/java/com/team2813/subsystems/Intake.java rename to src/main/java/com/team2813/subsystems/intake/IntakeSubsystem.java index 7b14dc18..9a785ca8 100644 --- a/src/main/java/com/team2813/subsystems/Intake.java +++ b/src/main/java/com/team2813/subsystems/intake/IntakeSubsystem.java @@ -1,4 +1,4 @@ -package com.team2813.subsystems; +package com.team2813.subsystems.intake; import static com.team2813.Constants.INTAKE_WHEEL; @@ -9,28 +9,31 @@ import com.team2813.lib2813.control.PIDMotor; import com.team2813.lib2813.control.motors.TalonFXWrapper; import com.team2813.lib2813.util.ConfigUtils; +import com.team2813.subsystems.ParameterizedIntakeSubsystem; import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; /** This is the Intake. His name is Joe. Please be kind to him and say hi. Have a nice day! */ -public final class Intake extends ParameterizedIntakeSubsystem { +class IntakeSubsystem extends ParameterizedIntakeSubsystem implements Intake { static final Params PARAMS = Params.builder().setIntakeDemand(4).setOuttakeDemand(-3).build(); static final double BUMP_VOLTAGE = -4; private final DigitalInput beamBreak; - public Intake(NetworkTableInstance networkTableInstance) { + public IntakeSubsystem(NetworkTableInstance networkTableInstance) { this( new TalonFXWrapper(INTAKE_WHEEL, InvertType.CLOCKWISE), new DigitalInput(1), networkTableInstance); } - Intake(PIDMotor motor, DigitalInput beamBreak, NetworkTableInstance networkTableInstance) { + IntakeSubsystem( + PIDMotor motor, DigitalInput beamBreak, NetworkTableInstance networkTableInstance) { super(motor, PARAMS); this.beamBreak = beamBreak; if (motor instanceof TalonFXWrapper wrapper) { @@ -46,14 +49,34 @@ public Intake(NetworkTableInstance networkTableInstance) { hasCoralPublisher = networkTable.getBooleanTopic("Has Coral").publish(); } + void intakeCoral() { + super.intakeGamePiece(); + } + + void outtakeCoral() { + super.outtakeGamePiece(); + } + + @Override + public Subsystem asSubsystem() { + return this; + } + + @Override public Command bumpAlgaeCommand() { return setMotorDemandCommand(BUMP_VOLTAGE); } + void bumpAlgae() { + setMotorDemand(BUMP_VOLTAGE); + } + + @Override public Command slowOuttakeItemCommand() { return setMotorDemandCommand(0.75 * PARAMS.outtakeDemand()); } + @Override public boolean hasCoral() { return !beamBreak.get(); } diff --git a/src/test/java/com/team2813/subsystems/IntakeTest.java b/src/test/java/com/team2813/subsystems/IntakeTest.java deleted file mode 100644 index afbd33a8..00000000 --- a/src/test/java/com/team2813/subsystems/IntakeTest.java +++ /dev/null @@ -1,143 +0,0 @@ -package com.team2813.subsystems; - -import static com.google.common.truth.Truth.assertThat; -import static com.team2813.subsystems.Intake.BUMP_VOLTAGE; -import static org.mockito.Mockito.mock; -import static org.mockito.Mockito.verifyNoInteractions; -import static org.mockito.Mockito.when; - -import com.team2813.IsolatedNetworkTablesExtension; -import com.team2813.lib2813.testing.junit.jupiter.CommandTester; -import com.team2813.lib2813.testing.junit.jupiter.WPILibExtension; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.Command; -import org.junit.jupiter.api.Test; -import org.junit.jupiter.api.extension.ExtendWith; -import org.mockito.Answers; - -@ExtendWith({IsolatedNetworkTablesExtension.class, WPILibExtension.class}) -public final class IntakeTest { - final FakePIDMotor fakeMotor = mock(FakePIDMotor.class, Answers.CALLS_REAL_METHODS); - final DigitalInput mockBeamBreak = mock(DigitalInput.class); - - @Test - public void constructRealInstance(NetworkTableInstance ntInstance) { - try (Intake ignored = new Intake(ntInstance)) { - assertThat(fakeMotor.demand).isWithin(0.01).of(0.0); - } - } - - @Test - public void initialState(NetworkTableInstance ntInstance) { - try (Intake ignored = new Intake(fakeMotor, mockBeamBreak, ntInstance)) { - assertThat(fakeMotor.demand).isWithin(0.01).of(0.0); - verifyNoInteractions(fakeMotor); - } - } - - @Test - public void intakeCoral(NetworkTableInstance ntInstance, CommandTester commandTester) { - try (Intake intake = new Intake(fakeMotor, mockBeamBreak, ntInstance)) { - Command command = intake.intakeItemCommand(); - assertThat(fakeMotor.demand).isWithin(0.01).of(0.0); - - commandTester.runUntilComplete(command); - - assertThat(fakeMotor.getVoltage()).isWithin(0.01).of(Intake.PARAMS.intakeDemand()); - } - } - - @Test - public void stopAfterIntakingCoral(NetworkTableInstance ntInstance, CommandTester commandTester) { - try (Intake intake = new Intake(fakeMotor, mockBeamBreak, ntInstance)) { - Command command = intake.intakeItemCommand(); - commandTester.runUntilComplete(command); - command = intake.stopMotorCommand(); - assertMotorIsRunning(); - - commandTester.runUntilComplete(command); - - assertMotorIsStopped(); - } - } - - @Test - public void outtakeCoral(NetworkTableInstance ntInstance, CommandTester commandTester) { - try (Intake intake = new Intake(fakeMotor, mockBeamBreak, ntInstance)) { - intake.intakeGamePiece(); - Command command = intake.outtakeItemCommand(); - assertMotorIsRunning(); - - commandTester.runUntilComplete(command); - - assertThat(fakeMotor.getVoltage()).isWithin(0.01).of(Intake.PARAMS.outtakeDemand()); - } - } - - @Test - public void stopAfterOutakingCoral(NetworkTableInstance ntInstance, CommandTester commandTester) { - try (Intake intake = new Intake(fakeMotor, mockBeamBreak, ntInstance)) { - intake.intakeGamePiece(); - Command command = intake.outtakeItemCommand(); - commandTester.runUntilComplete(command); - command = intake.stopMotorCommand(); - assertMotorIsRunning(); - - commandTester.runUntilComplete(command); - - assertMotorIsStopped(); - } - } - - @Test - public void bumpAlgae(NetworkTableInstance ntInstance, CommandTester commandTester) { - try (Intake intake = new Intake(fakeMotor, mockBeamBreak, ntInstance)) { - intake.intakeGamePiece(); - Command command = intake.bumpAlgaeCommand(); - assertMotorIsRunning(); - - commandTester.runUntilComplete(command); - - assertThat(fakeMotor.getVoltage()).isWithin(0.01).of(BUMP_VOLTAGE); - } - } - - @Test - public void stopAfterBumpingAlgae(NetworkTableInstance ntInstance, CommandTester commandTester) { - try (Intake intake = new Intake(fakeMotor, mockBeamBreak, ntInstance)) { - Command command = intake.bumpAlgaeCommand(); - commandTester.runUntilComplete(command); - command = intake.stopMotorCommand(); - assertMotorIsRunning(); - - commandTester.runUntilComplete(command); - - assertMotorIsStopped(); - } - } - - @Test - public void hasCoral_withCoral(NetworkTableInstance ntInstance) { - try (Intake intake = new Intake(fakeMotor, mockBeamBreak, ntInstance)) { - when(mockBeamBreak.get()).thenReturn(false); - assertThat(intake.hasCoral()).isTrue(); - } - } - - @Test - public void hasCoral_withoutCoral(NetworkTableInstance ntInstance) { - try (Intake intake = new Intake(fakeMotor, mockBeamBreak, ntInstance)) { - when(mockBeamBreak.get()).thenReturn(true); - assertThat(intake.hasCoral()).isFalse(); - } - } - - private void assertMotorIsStopped() { - assertThat(fakeMotor.demand).isWithin(0.01).of(0.0); - } - - private void assertMotorIsRunning() { - assertThat(fakeMotor.demand).isNotWithin(0.01).of(0.0); - } -} diff --git a/src/test/java/com/team2813/subsystems/intake/IntakeSubsystemTest.java b/src/test/java/com/team2813/subsystems/intake/IntakeSubsystemTest.java new file mode 100644 index 00000000..8d852055 --- /dev/null +++ b/src/test/java/com/team2813/subsystems/intake/IntakeSubsystemTest.java @@ -0,0 +1,105 @@ +package com.team2813.subsystems.intake; + +import static com.google.common.truth.Truth.assertThat; +import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.verifyNoInteractions; + +import com.team2813.IsolatedNetworkTablesExtension; +import com.team2813.lib2813.control.ControlMode; +import com.team2813.lib2813.control.PIDMotor; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DigitalInput; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.extension.ExtendWith; +import org.mockito.Answers; + +@ExtendWith(IsolatedNetworkTablesExtension.class) +public final class IntakeSubsystemTest { + final FakePIDMotor fakeMotor = mock(FakePIDMotor.class, Answers.CALLS_REAL_METHODS); + final DigitalInput mockBeamBreak = mock(DigitalInput.class); + + abstract static class FakePIDMotor implements PIDMotor { + double dutyCycle = 0.0f; + + @Override + public void set(ControlMode mode, double demand) { + assertThat(mode).isEqualTo(ControlMode.VOLTAGE); + this.dutyCycle = demand; + } + } + + @Test + public void constructRealInstance(NetworkTableInstance ntInstance) { + try (IntakeSubsystem intake = new IntakeSubsystem(ntInstance)) { + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + } + } + + @Test + public void initialState(NetworkTableInstance ntInstance) { + try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + verifyNoInteractions(fakeMotor); + } + } + + @Test + public void intakeCoral(NetworkTableInstance ntInstance) { + try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.intakeCoral(); + + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(IntakeSubsystem.PARAMS.intakeDemand()); + } + } + + @Test + public void stopAfterIntakingCoral(NetworkTableInstance ntInstance) { + try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.intakeCoral(); + + intake.stopMotor(); + + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + } + } + + @Test + public void outtakeCoral(NetworkTableInstance ntInstance) { + try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.outtakeCoral(); + + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(IntakeSubsystem.PARAMS.outtakeDemand()); + } + } + + @Test + public void stopAfterOuttakingCoral(NetworkTableInstance ntInstance) { + try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.outtakeCoral(); + + intake.stopMotor(); + + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + } + } + + @Test + public void bumpAlgae(NetworkTableInstance ntInstance) { + try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.bumpAlgae(); + + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(IntakeSubsystem.BUMP_VOLTAGE); + } + } + + @Test + public void stopAfterBumpingAlgae(NetworkTableInstance ntInstance) { + try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.bumpAlgae(); + + intake.stopMotor(); + + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + } + } +} From 1b3e1b88db3174c1e1ae86245e854a8504cfd54b Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Wed, 26 Mar 2025 10:42:53 -0700 Subject: [PATCH 03/11] Update IntakeSubsystemTest to use WPILibExtension --- .../intake/IntakeSubsystemTest.java | 71 +++++++++++++------ 1 file changed, 49 insertions(+), 22 deletions(-) diff --git a/src/test/java/com/team2813/subsystems/intake/IntakeSubsystemTest.java b/src/test/java/com/team2813/subsystems/intake/IntakeSubsystemTest.java index 8d852055..8e31d99c 100644 --- a/src/test/java/com/team2813/subsystems/intake/IntakeSubsystemTest.java +++ b/src/test/java/com/team2813/subsystems/intake/IntakeSubsystemTest.java @@ -7,13 +7,16 @@ import com.team2813.IsolatedNetworkTablesExtension; import com.team2813.lib2813.control.ControlMode; import com.team2813.lib2813.control.PIDMotor; +import com.team2813.lib2813.testing.junit.jupiter.CommandTester; +import com.team2813.lib2813.testing.junit.jupiter.WPILibExtension; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; import org.junit.jupiter.api.Test; import org.junit.jupiter.api.extension.ExtendWith; import org.mockito.Answers; -@ExtendWith(IsolatedNetworkTablesExtension.class) +@ExtendWith({IsolatedNetworkTablesExtension.class, WPILibExtension.class}) public final class IntakeSubsystemTest { final FakePIDMotor fakeMotor = mock(FakePIDMotor.class, Answers.CALLS_REAL_METHODS); final DigitalInput mockBeamBreak = mock(DigitalInput.class); @@ -44,62 +47,86 @@ public void initialState(NetworkTableInstance ntInstance) { } @Test - public void intakeCoral(NetworkTableInstance ntInstance) { - try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { - intake.intakeCoral(); + public void intakeCoral(NetworkTableInstance ntInstance, CommandTester CommandTesterInterface) { + try (var intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + Command command = intake.intakeItemCommand(); + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + + CommandTesterInterface.runUntilComplete(command); assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(IntakeSubsystem.PARAMS.intakeDemand()); } } @Test - public void stopAfterIntakingCoral(NetworkTableInstance ntInstance) { - try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { - intake.intakeCoral(); + public void stopAfterIntakingCoral( + NetworkTableInstance ntInstance, CommandTester CommandTesterInterface) { + try (var intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + Command command = intake.intakeItemCommand(); + CommandTesterInterface.runUntilComplete(command); + command = intake.stopMotorCommand(); + assertThat(fakeMotor.dutyCycle).isGreaterThan(0.01); - intake.stopMotor(); + CommandTesterInterface.runUntilComplete(command); assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); } } @Test - public void outtakeCoral(NetworkTableInstance ntInstance) { - try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { - intake.outtakeCoral(); + public void outtakeCoral(NetworkTableInstance ntInstance, CommandTester CommandTesterInterface) { + try (var intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.intakeCoral(); + Command command = intake.outtakeItemCommand(); + assertThat(fakeMotor.dutyCycle).isGreaterThan(0.01); + + CommandTesterInterface.runUntilComplete(command); assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(IntakeSubsystem.PARAMS.outtakeDemand()); } } @Test - public void stopAfterOuttakingCoral(NetworkTableInstance ntInstance) { - try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { - intake.outtakeCoral(); + public void stopAfterOuttakingCoral( + NetworkTableInstance ntInstance, CommandTester CommandTesterInterface) { + try (var intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.intakeCoral(); + Command command = intake.outtakeItemCommand(); + CommandTesterInterface.runUntilComplete(command); + command = intake.stopMotorCommand(); - intake.stopMotor(); + CommandTesterInterface.runUntilComplete(command); assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0); } } @Test - public void bumpAlgae(NetworkTableInstance ntInstance) { - try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { - intake.bumpAlgae(); + public void bumpAlgae(NetworkTableInstance ntInstance, CommandTester CommandTesterInterface) { + try (var intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.intakeCoral(); + Command command = intake.bumpAlgaeCommand(); + assertThat(fakeMotor.dutyCycle).isGreaterThan(0.01); + + CommandTesterInterface.runUntilComplete(command); assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(IntakeSubsystem.BUMP_VOLTAGE); } } @Test - public void stopAfterBumpingAlgae(NetworkTableInstance ntInstance) { - try (IntakeSubsystem intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { - intake.bumpAlgae(); + public void stopAfterBumpingAlgae( + NetworkTableInstance ntInstance, CommandTester CommandTesterInterface) { + try (var intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + Command command = intake.bumpAlgaeCommand(); + CommandTesterInterface.runUntilComplete(command); + command = intake.stopMotorCommand(); - intake.stopMotor(); + CommandTesterInterface.runUntilComplete(command); assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0); } } } From dbdbb7514748fc1536e8ae8e77254ec7bfe2854b Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 30 Mar 2025 08:38:06 -0700 Subject: [PATCH 04/11] Add Climb interface --- .../java/com/team2813/RobotContainer.java | 16 +++++------ .../com/team2813/subsystems/climb/Climb.java | 20 +++++++++++++ .../{Climb.java => climb/ClimbSubsystem.java} | 28 ++++++++++++++++--- .../subsystems/elevator/Elevator.java | 4 +-- .../team2813/subsystems/intake/Intake.java | 8 ++---- 5 files changed, 55 insertions(+), 21 deletions(-) create mode 100644 src/main/java/com/team2813/subsystems/climb/Climb.java rename src/main/java/com/team2813/subsystems/{Climb.java => climb/ClimbSubsystem.java} (74%) diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 8d5ee0b3..16b6ccfe 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -21,6 +21,7 @@ import com.team2813.lib2813.limelight.BotPoseEstimate; import com.team2813.lib2813.limelight.Limelight; import com.team2813.subsystems.*; +import com.team2813.subsystems.climb.Climb; import com.team2813.subsystems.elevator.Elevator; import com.team2813.subsystems.intake.Intake; import com.team2813.sysid.*; @@ -58,9 +59,9 @@ public class RobotContainer implements AutoCloseable { public RobotContainer(ShuffleboardTabs shuffleboard, NetworkTableInstance networkTableInstance) { var localization = new RobotLocalization(networkTableInstance); this.drive = new Drive(networkTableInstance, localization); - this.elevator = Elevator.create(() -> -OPERATOR_CONTROLLER.getRightY()); + this.elevator = Elevator.create(networkTableInstance, () -> -OPERATOR_CONTROLLER.getRightY()); this.intakePivot = new IntakePivot(networkTableInstance); - this.climb = new Climb(networkTableInstance); + this.climb = Climb.create(networkTableInstance); this.intake = Intake.create(networkTableInstance); this.groundIntakePivot = new GroundIntakePivot(networkTableInstance); autoChooser = @@ -417,14 +418,11 @@ private void configureBindings(RobotLocalization localization) { intakePivot::atPosition, () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), intakePivot), - new InstantCommand(climb::lower, climb))); - CLIMB_DOWN.onFalse(new InstantCommand(climb::stop, climb)); + climb.lowerCommand())); + CLIMB_DOWN.onFalse(climb.stopCommand()); - CLIMB_UP.whileTrue( - new SequentialCommandGroup( - new LockFunctionCommand(climb::limitSwitchPressed, climb::raise, climb), - new InstantCommand(climb::stop, climb))); - CLIMB_UP.onFalse(new InstantCommand(climb::stop, climb)); + CLIMB_UP.whileTrue(climb.raiseCommand()); + CLIMB_UP.onFalse(climb.stopCommand()); ALGAE_BUMP.whileTrue( new SequentialCommandGroup( diff --git a/src/main/java/com/team2813/subsystems/climb/Climb.java b/src/main/java/com/team2813/subsystems/climb/Climb.java new file mode 100644 index 00000000..ac3bd139 --- /dev/null +++ b/src/main/java/com/team2813/subsystems/climb/Climb.java @@ -0,0 +1,20 @@ +package com.team2813.subsystems.climb; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.Command; + +public interface Climb extends AutoCloseable { + + static Climb create(NetworkTableInstance networkTableInstance) { + return new ClimbSubsystem(networkTableInstance); + } + + Command stopCommand(); + + Command raiseCommand(); + + Command lowerCommand(); + + @Override + void close(); +} diff --git a/src/main/java/com/team2813/subsystems/Climb.java b/src/main/java/com/team2813/subsystems/climb/ClimbSubsystem.java similarity index 74% rename from src/main/java/com/team2813/subsystems/Climb.java rename to src/main/java/com/team2813/subsystems/climb/ClimbSubsystem.java index 9cf4b979..f9ab6352 100644 --- a/src/main/java/com/team2813/subsystems/Climb.java +++ b/src/main/java/com/team2813/subsystems/climb/ClimbSubsystem.java @@ -1,4 +1,4 @@ -package com.team2813.subsystems; +package com.team2813.subsystems.climb; import static com.team2813.Constants.CLIMB_1; import static com.team2813.Constants.CLIMB_2; @@ -6,6 +6,7 @@ import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.team2813.commands.LockFunctionCommand; import com.team2813.lib2813.control.ControlMode; import com.team2813.lib2813.control.InvertType; import com.team2813.lib2813.control.PIDMotor; @@ -15,15 +16,18 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** This is the Climb. Her name is Lisa. Please be kind to her and say hi. Have a nice day! */ -public class Climb extends SubsystemBase implements AutoCloseable { +final class ClimbSubsystem extends SubsystemBase implements AutoCloseable, Climb { private final PIDMotor climbMotor1; private final DigitalInput limitSwitch; - public Climb(NetworkTableInstance networkTableInstance) { + public ClimbSubsystem(NetworkTableInstance networkTableInstance) { limitSwitch = new DigitalInput(0); TalonFXWrapper climbMotor1 = new TalonFXWrapper(CLIMB_1, InvertType.CLOCKWISE); climbMotor1.motor().setNeutralMode(NeutralModeValue.Brake); @@ -43,6 +47,22 @@ public Climb(NetworkTableInstance networkTableInstance) { limitSwitchPressed = networkTable.getBooleanTopic("limit switch pressed").publish(); } + @Override + public Command stopCommand() { + return new InstantCommand(this::stop, this); + } + + @Override + public Command raiseCommand() { + return new SequentialCommandGroup( + new LockFunctionCommand(this::limitSwitchPressed, this::raise, this), stopCommand()); + } + + @Override + public Command lowerCommand() { + return new InstantCommand(this::lower, this); + } + public void raise() { if (!limitSwitchPressed()) { climbMotor1.set(ControlMode.VOLTAGE, -5); @@ -53,7 +73,7 @@ public void lower() { climbMotor1.set(ControlMode.VOLTAGE, 6); } - public void stop() { + private void stop() { climbMotor1.set(ControlMode.VOLTAGE, 0); } diff --git a/src/main/java/com/team2813/subsystems/elevator/Elevator.java b/src/main/java/com/team2813/subsystems/elevator/Elevator.java index 90656476..45536837 100644 --- a/src/main/java/com/team2813/subsystems/elevator/Elevator.java +++ b/src/main/java/com/team2813/subsystems/elevator/Elevator.java @@ -15,8 +15,8 @@ public interface Elevator { * * @param movement Controller input. */ - static Elevator create(DoubleSupplier movement) { - return new ElevatorSubsystem(NetworkTableInstance.getDefault(), movement); + static Elevator create(NetworkTableInstance networkTableInstance, DoubleSupplier movement) { + return new ElevatorSubsystem(networkTableInstance, movement); } /** diff --git a/src/main/java/com/team2813/subsystems/intake/Intake.java b/src/main/java/com/team2813/subsystems/intake/Intake.java index 67243c96..540c27d1 100644 --- a/src/main/java/com/team2813/subsystems/intake/Intake.java +++ b/src/main/java/com/team2813/subsystems/intake/Intake.java @@ -6,12 +6,8 @@ public interface Intake extends AutoCloseable { - static Intake create() { - return create(NetworkTableInstance.getDefault()); - } - - static Intake create(NetworkTableInstance ntInstance) { - return new IntakeSubsystem(ntInstance); + static Intake create(NetworkTableInstance networkTableInstance) { + return new IntakeSubsystem(networkTableInstance); } boolean hasCoral(); From dfb7543f3ae9a3ede7122a33cef69f1585686d3c Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 30 Mar 2025 21:32:11 -0700 Subject: [PATCH 05/11] Add IntakePivot interface --- src/main/java/com/team2813/Constants.java | 3 + .../java/com/team2813/RobotContainer.java | 98 +++++-------------- .../subsystems/intake/IntakePivot.java | 51 ++++++++++ .../IntakePivotSubsystem.java} | 53 +++++----- .../intake/ManualIntakePivot.java} | 19 ++-- 5 files changed, 114 insertions(+), 110 deletions(-) create mode 100644 src/main/java/com/team2813/subsystems/intake/IntakePivot.java rename src/main/java/com/team2813/subsystems/{IntakePivot.java => intake/IntakePivotSubsystem.java} (65%) rename src/main/java/com/team2813/{commands/ManuelIntakePivot.java => subsystems/intake/ManualIntakePivot.java} (54%) diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index a0ab4d42..11318e4b 100644 --- a/src/main/java/com/team2813/Constants.java +++ b/src/main/java/com/team2813/Constants.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.util.function.DoubleSupplier; public final class Constants { @@ -87,6 +88,8 @@ private OperatorConstants() { public static final Trigger MANUAL_GROUND_UP = OPERATOR_CONTROLLER.povLeft(); public static final Trigger MANUAL_GROUND_DOWN = OPERATOR_CONTROLLER.povRight(); public static final Trigger MANUAL_GROUND_STOW = OPERATOR_CONTROLLER.share(); + + public static final DoubleSupplier MANUAL_INTAKE_PIVOT_POS = OPERATOR_CONTROLLER::getLeftY; } private Constants() { diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 16b6ccfe..0d1cb1d8 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -15,7 +15,6 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.events.EventTrigger; import com.team2813.commands.LockFunctionCommand; -import com.team2813.commands.ManuelIntakePivot; import com.team2813.commands.OuttakeCommand; import com.team2813.commands.RobotLocalization; import com.team2813.lib2813.limelight.BotPoseEstimate; @@ -24,6 +23,7 @@ import com.team2813.subsystems.climb.Climb; import com.team2813.subsystems.elevator.Elevator; import com.team2813.subsystems.intake.Intake; +import com.team2813.subsystems.intake.IntakePivot; import com.team2813.sysid.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -60,7 +60,7 @@ public RobotContainer(ShuffleboardTabs shuffleboard, NetworkTableInstance networ var localization = new RobotLocalization(networkTableInstance); this.drive = new Drive(networkTableInstance, localization); this.elevator = Elevator.create(networkTableInstance, () -> -OPERATOR_CONTROLLER.getRightY()); - this.intakePivot = new IntakePivot(networkTableInstance); + this.intakePivot = IntakePivot.create(networkTableInstance); this.climb = Climb.create(networkTableInstance); this.intake = Intake.create(networkTableInstance); this.groundIntakePivot = new GroundIntakePivot(networkTableInstance); @@ -93,39 +93,30 @@ private static void configureAutoCommands( NamedCommands.registerCommand( "PrepareL2", new ParallelCommandGroup( - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), + intakePivot.setSetpointCommand(IntakePivot.Rotations.OUTTAKE), elevator.setSetpointCommand(Elevator.Position.BOTTOM))); NamedCommands.registerCommand( "PrepareL3", new ParallelCommandGroup( - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), + intakePivot.setSetpointCommand(IntakePivot.Rotations.OUTTAKE), elevator.setSetpointCommand(Elevator.Position.TOP))); NamedCommands.registerCommand( - "PrepareScore", - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot)); + "PrepareScore", intakePivot.setSetpointCommand(IntakePivot.Rotations.OUTTAKE)); NamedCommands.registerCommand( "ScoreL2", new SequentialCommandGroup( new ParallelCommandGroup( elevator.moveToPositionCommand(Elevator.Position.BOTTOM), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), - intakePivot) - .withTimeout(SECONDS_2)), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.OUTTAKE)), intake.slowOuttakeItemCommand(), new WaitCommand(DROP_CORAL), new ParallelCommandGroup( intake.stopMotorCommand(), elevator.disableCommand(), - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); + intakePivot.setSetpointCommand(IntakePivot.Rotations.INTAKE)))); NamedCommands.registerCommand( "ScoreL1", @@ -139,36 +130,26 @@ private static void configureAutoCommands( "ScoreL3", new SequentialCommandGroup( elevator.moveToPositionCommand(Elevator.Position.TOP), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), - intakePivot) - .withTimeout(SECONDS_2), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.OUTTAKE), intake.slowOuttakeItemCommand(), new WaitCommand(DROP_CORAL), new ParallelCommandGroup( intake.stopMotorCommand(), elevator.setSetpointCommand(Elevator.Position.BOTTOM), - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); + intakePivot.setSetpointCommand(IntakePivot.Rotations.INTAKE)))); NamedCommands.registerCommand( "BumpAlgaeLow", new SequentialCommandGroup( new ParallelCommandGroup( elevator.moveToPositionCommand(Elevator.Position.BOTTOM), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), - intakePivot) - .withTimeout(SECONDS_2)), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.ALGAE_BUMP)), intake.outtakeItemCommand(), new WaitCommand(SECONDS_1), // TODO: Wait until we bump low algae new ParallelCommandGroup( intake.stopMotorCommand(), elevator.setSetpointCommand(Elevator.Position.BOTTOM), - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); + intakePivot.setSetpointCommand(IntakePivot.Rotations.INTAKE)))); NamedCommands.registerCommand( "BumpAlgaeHigh", @@ -176,47 +157,36 @@ private static void configureAutoCommands( new ParallelCommandGroup( new SequentialCommandGroup( new WaitCommand(0.05), elevator.setSetpointCommand(Elevator.Position.TOP)), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), - intakePivot) - .withTimeout(SECONDS_2)), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.ALGAE_BUMP)), intake.bumpAlgaeCommand(), new WaitCommand(SECONDS_1), // TODO: Wait until we bump high algae new ParallelCommandGroup( intake.stopMotorCommand(), elevator.setSetpointCommand(Elevator.Position.BOTTOM), - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); + intakePivot.setSetpointCommand(IntakePivot.Rotations.INTAKE)))); NamedCommands.registerCommand( "IntakeCoral", new SequentialCommandGroup( new ParallelCommandGroup( elevator.moveToPositionCommand(Elevator.Position.BOTTOM), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), - intakePivot) - .withTimeout(SECONDS_2)), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.INTAKE)), intake.intakeItemCommand(), new WaitUntilCommand(intake::hasCoral).withTimeout(INTAKE_TIME), new ParallelCommandGroup( intake.intakeItemCommand(), elevator.disableCommand(), - new InstantCommand(intakePivot::disable, intakePivot)))); + intakePivot.disableCommand()))); new EventTrigger("PrepareL2") .onTrue( new ParallelCommandGroup( - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), + intakePivot.setSetpointCommand(IntakePivot.Rotations.OUTTAKE), elevator.setSetpointCommand(Elevator.Position.BOTTOM))); new EventTrigger("PrepareL3") .onTrue( new ParallelCommandGroup( - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), + intakePivot.setSetpointCommand(IntakePivot.Rotations.OUTTAKE), elevator.setSetpointCommand(Elevator.Position.TOP))); } @@ -355,12 +325,8 @@ private void configureBindings(RobotLocalization localization) { new SequentialCommandGroup( new ParallelCommandGroup( elevator.moveToPositionCommand(Elevator.Position.BOTTOM), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), - intakePivot) - .withTimeout(Units.Seconds.of(0.5))), - intake.intakeItemCommand())); + intakePivot.moveToPositionCommand(IntakePivot.Rotations.INTAKE), + intake.intakeItemCommand()))); INTAKE_BUTTON.onFalse(intake.stopMotorCommand()); GROUND_CORAL_INTAKE.whileTrue( @@ -394,30 +360,19 @@ private void configureBindings(RobotLocalization localization) { PREP_L2_CORAL.onTrue( new ParallelCommandGroup( elevator.moveToPositionCommand(Elevator.Position.BOTTOM), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), - intakePivot))); + intakePivot.moveToPositionCommand(IntakePivot.Rotations.OUTTAKE))); PREP_L3_CORAL.onTrue( new ParallelCommandGroup( elevator.moveToPositionCommand(Elevator.Position.TOP), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), - intakePivot))); + intakePivot.moveToPositionCommand(IntakePivot.Rotations.OUTTAKE))); /* Is there code for algea intake? R2.whileTrue( new InstantCommand() );*/ - intakePivot.setDefaultCommand( - new ManuelIntakePivot(intakePivot, () -> -OPERATOR_CONTROLLER.getLeftY())); CLIMB_DOWN.onTrue( new SequentialCommandGroup( - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), - intakePivot), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.ALGAE_BUMP), climb.lowerCommand())); CLIMB_DOWN.onFalse(climb.stopCommand()); @@ -426,16 +381,11 @@ private void configureBindings(RobotLocalization localization) { ALGAE_BUMP.whileTrue( new SequentialCommandGroup( - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), - intakePivot) - .withTimeout(Units.Seconds.of(2)), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.ALGAE_BUMP), intake.bumpAlgaeCommand())); ALGAE_BUMP.onFalse( new ParallelCommandGroup( - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), + intakePivot.setSetpointCommand(IntakePivot.Rotations.OUTTAKE), intake.stopMotorCommand())); SLOW_OUTTAKE.onTrue(intake.slowOuttakeItemCommand()); diff --git a/src/main/java/com/team2813/subsystems/intake/IntakePivot.java b/src/main/java/com/team2813/subsystems/intake/IntakePivot.java new file mode 100644 index 00000000..f133de33 --- /dev/null +++ b/src/main/java/com/team2813/subsystems/intake/IntakePivot.java @@ -0,0 +1,51 @@ +package com.team2813.subsystems.intake; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.Supplier; + +public interface IntakePivot { + + static IntakePivot create(NetworkTableInstance networkTableInstance) { + return new IntakePivotSubsystem(networkTableInstance); + } + + /** + * Returns a command that sets the setpoint and waits for the elevator to reach that point. + * + * @param position The desired position. + */ + Command moveToPositionCommand(Rotations position); + + /** + * Returns a command that sets the setpoint to the given value. + * + *

The returned command will complete immediately after executing. The default elevator command + * will move the elevator to the given setpoint. + * + * @param position The desired position. + */ + Command setSetpointCommand(Rotations position); + + Command disableCommand(); + + enum Rotations implements Supplier { + OUTTAKE(Units.Rotations.of(0.723389)), // TODO: NEEDS TUNING + INTAKE(Units.Rotations.of(0.448721)), // TODO: NEEDS TUNING + ALGAE_BUMP(Units.Rotations.of(1.108418)), + HARD_STOP(Units.Rotations.of(0.438721)); // TODO: NEEDS TUNING + + Rotations(Angle pos) { + this.pos = pos; + } + + private final Angle pos; + + @Override + public Angle get() { + return pos; + } + } +} diff --git a/src/main/java/com/team2813/subsystems/IntakePivot.java b/src/main/java/com/team2813/subsystems/intake/IntakePivotSubsystem.java similarity index 65% rename from src/main/java/com/team2813/subsystems/IntakePivot.java rename to src/main/java/com/team2813/subsystems/intake/IntakePivotSubsystem.java index b872778e..6aef5436 100644 --- a/src/main/java/com/team2813/subsystems/IntakePivot.java +++ b/src/main/java/com/team2813/subsystems/intake/IntakePivotSubsystem.java @@ -1,6 +1,9 @@ -package com.team2813.subsystems; +package com.team2813.subsystems.intake; + +import static com.team2813.Constants.OperatorConstants.MANUAL_INTAKE_PIVOT_POS; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.team2813.commands.LockFunctionCommand; import com.team2813.lib2813.control.ControlMode; import com.team2813.lib2813.control.InvertType; import com.team2813.lib2813.control.PIDMotor; @@ -12,16 +15,19 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; -import java.util.function.Supplier; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; /** * This is the Intake Pivot. Her name is Sophie. Please be kind to her and say hi. Have a nice day! */ -public class IntakePivot extends MotorSubsystem { +class IntakePivotSubsystem extends MotorSubsystem + implements IntakePivot { + private static final Time MOVEMENT_TIMEOUT = Units.Seconds.of(2); - public IntakePivot(NetworkTableInstance networkTableInstance) { + IntakePivotSubsystem(NetworkTableInstance networkTableInstance) { super( new MotorSubsystemConfiguration( pivotMotor(), new CancoderWrapper(com.team2813.Constants.INTAKE_ENCODER)) @@ -34,11 +40,24 @@ public IntakePivot(NetworkTableInstance networkTableInstance) { NetworkTable networkTable = networkTableInstance.getTable("IntakePivot"); intakePivotPosition = networkTable.getDoubleTopic("position").publish(); atPosition = networkTable.getBooleanTopic("at position").publish(); + + setDefaultCommand(new ManualIntakePivot(this, () -> -MANUAL_INTAKE_PIVOT_POS.getAsDouble())); + } + + @Override + public Command setSetpointCommand(Rotations position) { + return new InstantCommand(() -> setSetpoint(position), this); + } + + @Override + public Command moveToPositionCommand(Rotations position) { + return new LockFunctionCommand(this::atPosition, () -> this.setSetpoint(position), this) + .withTimeout(MOVEMENT_TIMEOUT); } - @Deprecated - public void resetPosition() { - encoder.setPosition(Rotations.HARD_STOP.get()); + @Override + public Command disableCommand() { + return new InstantCommand(this::disable); } @Override @@ -68,22 +87,4 @@ public void periodic() { intakePivotPosition.set(getPositionMeasure().in(Units.Rotations)); atPosition.set(atPosition()); } - - public enum Rotations implements Supplier { - OUTTAKE(Units.Rotations.of(0.723389)), // TODO: NEEDS TUNING - INTAKE(Units.Rotations.of(0.448721)), // TODO: NEEDS TUNING - ALGAE_BUMP(Units.Rotations.of(1.108418)), - HARD_STOP(Units.Rotations.of(0.438721)); // TODO: NEEDS TUNING - - Rotations(Angle pos) { - this.pos = pos; - } - - private final Angle pos; - - @Override - public Angle get() { - return pos; - } - } } diff --git a/src/main/java/com/team2813/commands/ManuelIntakePivot.java b/src/main/java/com/team2813/subsystems/intake/ManualIntakePivot.java similarity index 54% rename from src/main/java/com/team2813/commands/ManuelIntakePivot.java rename to src/main/java/com/team2813/subsystems/intake/ManualIntakePivot.java index c0b0f74c..02ace3d4 100644 --- a/src/main/java/com/team2813/commands/ManuelIntakePivot.java +++ b/src/main/java/com/team2813/subsystems/intake/ManualIntakePivot.java @@ -1,29 +1,28 @@ -package com.team2813.commands; +package com.team2813.subsystems.intake; import com.team2813.lib2813.control.ControlMode; -import com.team2813.subsystems.IntakePivot; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.DoubleSupplier; -public class ManuelIntakePivot extends Command { +class ManualIntakePivot extends Command { - private final IntakePivot intakepivot; + private final IntakePivotSubsystem intakePivot; private final DoubleSupplier rotation; - public ManuelIntakePivot(IntakePivot intakepivot, DoubleSupplier rotation) { - this.intakepivot = intakepivot; + ManualIntakePivot(IntakePivotSubsystem intakePivot, DoubleSupplier rotation) { + this.intakePivot = intakePivot; this.rotation = rotation; - addRequirements(intakepivot); + addRequirements(intakePivot); } public void execute() { double val = rotation.getAsDouble(); if (Math.abs(val) > 0.1) { - intakepivot.set(ControlMode.DUTY_CYCLE, val * .1); - } else if (!intakepivot.isEnabled()) { + intakePivot.set(ControlMode.DUTY_CYCLE, val * .1); + } else if (!intakePivot.isEnabled()) { // An InstantCommand initiated the motor, and // PID controller is disabled; stop the elevator motors, potentially sliding down. - intakepivot.set(ControlMode.DUTY_CYCLE, 0); + intakePivot.set(ControlMode.DUTY_CYCLE, 0); } // ..else an InstantCommand initiated the motor. Leave it running ast the current speed } } From ccbf52fec4852f551a355e28dab2d2fa37e3936f Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 31 Mar 2025 08:39:24 -0700 Subject: [PATCH 06/11] Extract Drive interface --- .../java/com/team2813/RobotContainer.java | 32 ++++++------- .../team2813/commands/RobotLocalization.java | 2 +- .../drive/DefaultCommand.java} | 11 ++--- .../com/team2813/subsystems/drive/Drive.java | 46 +++++++++++++++++++ .../{Drive.java => drive/DriveSubsystem.java} | 42 ++++++++++------- .../SimulatedSwerveDrivetrain.java | 2 +- .../com/team2813/sysid/SubsystemRegistry.java | 16 +++++-- 7 files changed, 106 insertions(+), 45 deletions(-) rename src/main/java/com/team2813/{commands/DefaultDriveCommand.java => subsystems/drive/DefaultCommand.java} (75%) create mode 100644 src/main/java/com/team2813/subsystems/drive/Drive.java rename src/main/java/com/team2813/subsystems/{Drive.java => drive/DriveSubsystem.java} (96%) rename src/main/java/com/team2813/subsystems/{ => drive}/SimulatedSwerveDrivetrain.java (98%) diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 0d1cb1d8..66f6ee19 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -21,6 +21,8 @@ import com.team2813.lib2813.limelight.Limelight; import com.team2813.subsystems.*; import com.team2813.subsystems.climb.Climb; +import com.team2813.subsystems.drive.Drive; +import com.team2813.subsystems.drive.DriveSubsystem; import com.team2813.subsystems.elevator.Elevator; import com.team2813.subsystems.intake.Intake; import com.team2813.subsystems.intake.IntakePivot; @@ -39,7 +41,6 @@ import java.io.IOException; import java.util.ArrayList; import java.util.List; -import java.util.Set; import org.json.simple.parser.ParseException; public class RobotContainer implements AutoCloseable { @@ -58,7 +59,8 @@ public class RobotContainer implements AutoCloseable { public RobotContainer(ShuffleboardTabs shuffleboard, NetworkTableInstance networkTableInstance) { var localization = new RobotLocalization(networkTableInstance); - this.drive = new Drive(networkTableInstance, localization); + var registry = new SubsystemRegistry(); + this.drive = Drive.create(networkTableInstance, localization, registry); this.elevator = Elevator.create(networkTableInstance, () -> -OPERATOR_CONTROLLER.getRightY()); this.intakePivot = IntakePivot.create(networkTableInstance); this.climb = Climb.create(networkTableInstance); @@ -68,8 +70,7 @@ public RobotContainer(ShuffleboardTabs shuffleboard, NetworkTableInstance networ configureAuto(drive, elevator, intakePivot, intake, groundIntake, groundIntakePivot); SmartDashboard.putData("Auto Routine", autoChooser); sysIdRoutineSelector = - new SysIdRoutineSelector( - new SubsystemRegistry(Set.of(drive)), RobotContainer::getSysIdRoutines, shuffleboard); + new SysIdRoutineSelector(registry, RobotContainer::getSysIdRoutines, shuffleboard); configureBindings(localization); } @@ -226,7 +227,7 @@ private static SendableChooser configureAuto( .map(alliance -> alliance != ALLIANCE_USED_IN_PATHS) .orElse(false); }, - drive // Reference to this subsystem to set requirements + drive.asSubsystem() // Reference to this subsystem to set requirements ); configureAutoCommands(elevator, intakePivot, intake, groundIntake, groundIntakePivot); return AutoBuilder.buildAutoChooser(); @@ -248,10 +249,10 @@ private static List getSysIdRoutines(SubsystemRegistry registry) new SysIdRoutine.Mechanism( (v) -> registry - .getSubsystem(Drive.class) + .getSubsystem(DriveSubsystem.class) .runSysIdRequest(DRIVE_SYSID.withVoltage(v)), null, - registry.getSubsystem(Drive.class))))); + registry.getSubsystem(DriveSubsystem.class))))); routines.add( new DropdownEntry( "Drive-Steer Motor", @@ -261,10 +262,10 @@ private static List getSysIdRoutines(SubsystemRegistry registry) new SysIdRoutine.Mechanism( (v) -> registry - .getSubsystem(Drive.class) + .getSubsystem(DriveSubsystem.class) .runSysIdRequest(STEER_SYSID.withVoltage(v)), null, - registry.getSubsystem(Drive.class))))); + registry.getSubsystem(DriveSubsystem.class))))); routines.add( new DropdownEntry( "Drive-Slip Test (Forward Quasistatic only)", @@ -277,19 +278,18 @@ private static List getSysIdRoutines(SubsystemRegistry registry) new SysIdRoutine.Mechanism( (v) -> registry - .getSubsystem(Drive.class) + .getSubsystem(DriveSubsystem.class) .runSysIdRequest(DRIVE_SYSID.withVoltage(v)), null, - registry.getSubsystem(Drive.class))))); + registry.getSubsystem(DriveSubsystem.class))))); return routines; } private void configureBindings(RobotLocalization localization) { // Driver - SLOWMODE_BUTTON.whileTrue(new InstantCommand(() -> drive.enableSlowMode(true), drive)); - SLOWMODE_BUTTON.onFalse(new InstantCommand(() -> drive.enableSlowMode(false), drive)); - SLOWMODE_BUTTON.onTrue(new InstantCommand(() -> drive.enableSlowMode(true), drive)); - SLOWMODE_BUTTON.onFalse(new InstantCommand(() -> drive.enableSlowMode(false), drive)); + SLOWMODE_BUTTON.whileTrue(drive.enableSlowModeCommand(true)); + SLOWMODE_BUTTON.onFalse(drive.enableSlowModeCommand(false)); + SLOWMODE_BUTTON.onTrue(drive.enableSlowModeCommand(true)); SETPOSE.onTrue( new InstantCommand( () -> @@ -299,7 +299,7 @@ private void configureBindings(RobotLocalization localization) { .map(Pose3d::toPose2d) .map(RobotContainer::toBotposeBlue) .ifPresent(drive::setPose))); - RESET_POSE.onTrue(new InstantCommand(drive::resetPose, drive)); + RESET_POSE.onTrue(drive.resetPoseCommand()); // Every subsystem should be in the set; we don't know what subsystem will be controlled, so // assume we control all of them diff --git a/src/main/java/com/team2813/commands/RobotLocalization.java b/src/main/java/com/team2813/commands/RobotLocalization.java index 59c11b58..d847a253 100644 --- a/src/main/java/com/team2813/commands/RobotLocalization.java +++ b/src/main/java/com/team2813/commands/RobotLocalization.java @@ -9,7 +9,7 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.Waypoint; import com.team2813.lib2813.preferences.PersistedConfiguration; -import com.team2813.subsystems.Drive; +import com.team2813.subsystems.drive.Drive; import com.team2813.vision.VisionNetworkTables; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; diff --git a/src/main/java/com/team2813/commands/DefaultDriveCommand.java b/src/main/java/com/team2813/subsystems/drive/DefaultCommand.java similarity index 75% rename from src/main/java/com/team2813/commands/DefaultDriveCommand.java rename to src/main/java/com/team2813/subsystems/drive/DefaultCommand.java index b1b3ef7b..edf05fb9 100644 --- a/src/main/java/com/team2813/commands/DefaultDriveCommand.java +++ b/src/main/java/com/team2813/subsystems/drive/DefaultCommand.java @@ -1,17 +1,16 @@ -package com.team2813.commands; +package com.team2813.subsystems.drive; -import com.team2813.subsystems.Drive; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.Supplier; -public final class DefaultDriveCommand extends Command { - private final Drive drive; +final class DefaultCommand extends Command { + private final DriveSubsystem drive; private final Supplier xSupplier; private final Supplier ySupplier; private final Supplier rotationSupplier; - public DefaultDriveCommand( - Drive drive, + public DefaultCommand( + DriveSubsystem drive, Supplier xSupplier, Supplier ySupplier, Supplier rotationSupplier) { diff --git a/src/main/java/com/team2813/subsystems/drive/Drive.java b/src/main/java/com/team2813/subsystems/drive/Drive.java new file mode 100644 index 00000000..82b18ec7 --- /dev/null +++ b/src/main/java/com/team2813/subsystems/drive/Drive.java @@ -0,0 +1,46 @@ +package com.team2813.subsystems.drive; + +import com.team2813.commands.RobotLocalization; +import com.team2813.sysid.SubsystemRegistry; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public interface Drive extends AutoCloseable { + + static Drive create( + NetworkTableInstance networkTableInstance, + RobotLocalization localization, + SubsystemRegistry registry) { + DriveSubsystem drive = new DriveSubsystem(networkTableInstance, localization); + registry.addSubsystem(drive); + return drive; + } + + static boolean onRed() { + return DriverStation.getAlliance() + .map(alliance -> alliance == DriverStation.Alliance.Red) + .orElse(false); + } + + Subsystem asSubsystem(); + + Pose2d getPose(); + + void setPose(Pose2d pose2d); + + ChassisSpeeds getRobotRelativeSpeeds(); + + /** Handles autonomous driving. */ + void drive(ChassisSpeeds chassisSpeeds); + + @Override + void close(); + + Command enableSlowModeCommand(boolean enable); + + Command resetPoseCommand(); +} diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java similarity index 96% rename from src/main/java/com/team2813/subsystems/Drive.java rename to src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java index d5647f9b..19a9a959 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java @@ -1,4 +1,4 @@ -package com.team2813.subsystems; +package com.team2813.subsystems.drive; import static com.team2813.Constants.*; import static com.team2813.Constants.DriverConstants.DRIVER_CONTROLLER; @@ -23,7 +23,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentricFacingAngle; import com.google.auto.value.AutoBuilder; -import com.team2813.commands.DefaultDriveCommand; import com.team2813.commands.RobotLocalization; import com.team2813.lib2813.limelight.BotPoseEstimate; import com.team2813.lib2813.preferences.PersistedConfiguration; @@ -45,6 +44,8 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.List; import java.util.function.DoubleSupplier; @@ -56,7 +57,7 @@ import org.photonvision.targeting.PhotonTrackedTarget; /** This is the Drive. His name is Gary. Please be kind to him and say hi. Have a nice day! */ -public class Drive extends SubsystemBase implements AutoCloseable { +public class DriveSubsystem extends SubsystemBase implements Drive { private static final double DEFAULT_MAX_VELOCITY_METERS_PER_SECOND = 6; private static final double DEFAULT_MAX_ROTATIONS_PER_SECOND = 1.2; private static final Matrix PHOTON_MULTIPLE_TAG_STD_DEVS = @@ -141,7 +142,7 @@ PhotonPoseEstimator.PoseStrategy poseStrategy() { /** Creates a builder for {@code DriveConfiguration} with default values. */ public static Builder builder() { - return new AutoBuilder_Drive_Configuration_Builder() + return new AutoBuilder_DriveSubsystem_Configuration_Builder() .addLimelightMeasurement(true) .usePhotonVisionLocation(false) .maxRotationsPerSecond(DEFAULT_MAX_ROTATIONS_PER_SECOND) @@ -182,11 +183,11 @@ default Builder maxVelocityInMetersPerSecond(double value) { } } - public Drive(NetworkTableInstance networkTableInstance, RobotLocalization localization) { + DriveSubsystem(NetworkTableInstance networkTableInstance, RobotLocalization localization) { this(networkTableInstance, localization, Configuration.fromPreferences()); } - public Drive( + DriveSubsystem( NetworkTableInstance networkTableInstance, RobotLocalization localization, Configuration config) { @@ -342,6 +343,11 @@ public Drive( } } + @Override + public Subsystem asSubsystem() { + return this; + } + private Rotation3d getRotation3d() { if (simDrivetrain != null) { return simDrivetrain.getRotation3d(); @@ -350,7 +356,7 @@ private Rotation3d getRotation3d() { } private Command createDefaultCommand() { - return new DefaultDriveCommand( + return new DefaultCommand( this, () -> -modifyAxis(DRIVER_CONTROLLER.getLeftY()) * config.maxVelocity(), () -> -modifyAxis(DRIVER_CONTROLLER.getLeftX()) * config.maxVelocity(), @@ -385,17 +391,11 @@ private double getPosition(int moduleId) { private final FieldCentric fieldCentricApplier = new FieldCentric().withDriveRequestType(SwerveModule.DriveRequestType.Velocity); - public static boolean onRed() { - return DriverStation.getAlliance() - .map(alliance -> alliance == DriverStation.Alliance.Red) - .orElse(false); - } - private boolean correctRotation = false; // Note: This is used for teleop drive. public void drive(double xSpeed, double ySpeed, double rotation) { - double multiplier = onRed() && correctRotation ? -this.multiplier : this.multiplier; + double multiplier = Drive.onRed() && correctRotation ? -this.multiplier : this.multiplier; drivetrain.setControl( fieldCentricApplier .withVelocityX(xSpeed * multiplier) @@ -409,6 +409,7 @@ public void runSysIdRequest(SwerveSysidRequest request) { } // Note: This is used for auto drive. + @Override public void drive(ChassisSpeeds demand) { drivetrain.setControl(applyRobotSpeedsApplier.withSpeeds(demand)); } @@ -438,6 +439,7 @@ public void setRotationVelocity(AngularVelocity rotationRate) { drivetrain.setControl(fieldCentricApplier.withRotationalRate(rotationRate)); } + @Override public Pose2d getPose() { return drivetrain.getState().Pose; } @@ -454,6 +456,7 @@ public void resetPose() { *

This is called by PathPlanner when it starts controlling the robot. It assumes that the * passed-in pose is correct. */ + @Override public void setPose(Pose2d pose) { correctRotation = true; if (pose != null) { @@ -471,6 +474,7 @@ public void setPose(Pose2d pose) { } } + @Override public ChassisSpeeds getRobotRelativeSpeeds() { return this.drivetrain.getKinematics().toChassisSpeeds(this.drivetrain.getState().ModuleStates); } @@ -554,8 +558,9 @@ public void simulationPeriodic() { simVisionSystem.update(drivePose); } - public void enableSlowMode(boolean enable) { - multiplier = enable ? 0.3 : 1; + @Override + public Command enableSlowModeCommand(boolean enable) { + return new InstantCommand(() -> multiplier = enable ? 0.3 : 1, this); } @Override @@ -563,4 +568,9 @@ public void close() { drivetrain.close(); photonPoseEstimator.close(); } + + @Override + public Command resetPoseCommand() { + return new InstantCommand(this::resetPose, this); + } } diff --git a/src/main/java/com/team2813/subsystems/SimulatedSwerveDrivetrain.java b/src/main/java/com/team2813/subsystems/drive/SimulatedSwerveDrivetrain.java similarity index 98% rename from src/main/java/com/team2813/subsystems/SimulatedSwerveDrivetrain.java rename to src/main/java/com/team2813/subsystems/drive/SimulatedSwerveDrivetrain.java index b4666a09..ba07e98c 100644 --- a/src/main/java/com/team2813/subsystems/SimulatedSwerveDrivetrain.java +++ b/src/main/java/com/team2813/subsystems/drive/SimulatedSwerveDrivetrain.java @@ -1,4 +1,4 @@ -package com.team2813.subsystems; +package com.team2813.subsystems.drive; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SimSwerveDrivetrain; diff --git a/src/main/java/com/team2813/sysid/SubsystemRegistry.java b/src/main/java/com/team2813/sysid/SubsystemRegistry.java index 10f65054..412f5919 100644 --- a/src/main/java/com/team2813/sysid/SubsystemRegistry.java +++ b/src/main/java/com/team2813/sysid/SubsystemRegistry.java @@ -4,21 +4,27 @@ import java.util.*; public final class SubsystemRegistry { - private final Map, Subsystem> subsystems; + private final Map, Subsystem> subsystemMap = new HashMap<>(); + + public SubsystemRegistry() {} public SubsystemRegistry(Collection subsystems) { - Map, Subsystem> subsystemMap = new HashMap<>(); for (Subsystem subsystem : subsystems) { subsystemMap.put(subsystem.getClass(), subsystem); } - this.subsystems = subsystemMap; + } + + public void addSubsystem(Subsystem subsystem) { + if (subsystemMap.putIfAbsent(subsystem.getClass(), subsystem) != null) { + throw new IllegalArgumentException("Instance already registered for " + subsystem); + } } public T getSubsystem(Class cls) { - return Objects.requireNonNull(cls.cast(this.subsystems.get(cls))); + return Objects.requireNonNull(cls.cast(this.subsystemMap.get(cls))); } public Set allSubsystems() { - return Set.copyOf(subsystems.values()); + return Set.copyOf(subsystemMap.values()); } } From 64a824f1a93e7e6137d812ad08e9f5904c61242a Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 31 Mar 2025 09:55:17 -0700 Subject: [PATCH 07/11] Remove RobotLocalization to com.team2813.subsystems.drive --- src/main/java/com/team2813/RobotContainer.java | 8 +++----- .../java/com/team2813/subsystems/drive/Drive.java | 11 ++++++----- .../com/team2813/subsystems/drive/DriveSubsystem.java | 11 ++++++++++- .../drive}/RobotLocalization.java | 5 ++--- 4 files changed, 21 insertions(+), 14 deletions(-) rename src/main/java/com/team2813/{commands => subsystems/drive}/RobotLocalization.java (98%) diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 66f6ee19..a4d96340 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -16,7 +16,6 @@ import com.pathplanner.lib.events.EventTrigger; import com.team2813.commands.LockFunctionCommand; import com.team2813.commands.OuttakeCommand; -import com.team2813.commands.RobotLocalization; import com.team2813.lib2813.limelight.BotPoseEstimate; import com.team2813.lib2813.limelight.Limelight; import com.team2813.subsystems.*; @@ -58,9 +57,8 @@ public class RobotContainer implements AutoCloseable { private final SysIdRoutineSelector sysIdRoutineSelector; public RobotContainer(ShuffleboardTabs shuffleboard, NetworkTableInstance networkTableInstance) { - var localization = new RobotLocalization(networkTableInstance); var registry = new SubsystemRegistry(); - this.drive = Drive.create(networkTableInstance, localization, registry); + this.drive = Drive.create(networkTableInstance, registry); this.elevator = Elevator.create(networkTableInstance, () -> -OPERATOR_CONTROLLER.getRightY()); this.intakePivot = IntakePivot.create(networkTableInstance); this.climb = Climb.create(networkTableInstance); @@ -71,7 +69,7 @@ public RobotContainer(ShuffleboardTabs shuffleboard, NetworkTableInstance networ SmartDashboard.putData("Auto Routine", autoChooser); sysIdRoutineSelector = new SysIdRoutineSelector(registry, RobotContainer::getSysIdRoutines, shuffleboard); - configureBindings(localization); + configureBindings(); } /** @@ -285,7 +283,7 @@ private static List getSysIdRoutines(SubsystemRegistry registry) return routines; } - private void configureBindings(RobotLocalization localization) { + private void configureBindings() { // Driver SLOWMODE_BUTTON.whileTrue(drive.enableSlowModeCommand(true)); SLOWMODE_BUTTON.onFalse(drive.enableSlowModeCommand(false)); diff --git a/src/main/java/com/team2813/subsystems/drive/Drive.java b/src/main/java/com/team2813/subsystems/drive/Drive.java index 82b18ec7..650b22c6 100644 --- a/src/main/java/com/team2813/subsystems/drive/Drive.java +++ b/src/main/java/com/team2813/subsystems/drive/Drive.java @@ -1,6 +1,5 @@ package com.team2813.subsystems.drive; -import com.team2813.commands.RobotLocalization; import com.team2813.sysid.SubsystemRegistry; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -11,10 +10,8 @@ public interface Drive extends AutoCloseable { - static Drive create( - NetworkTableInstance networkTableInstance, - RobotLocalization localization, - SubsystemRegistry registry) { + static Drive create(NetworkTableInstance networkTableInstance, SubsystemRegistry registry) { + RobotLocalization localization = new RobotLocalization(networkTableInstance); DriveSubsystem drive = new DriveSubsystem(networkTableInstance, localization); registry.addSubsystem(drive); return drive; @@ -43,4 +40,8 @@ static boolean onRed() { Command enableSlowModeCommand(boolean enable); Command resetPoseCommand(); + + Command rightAutoAlignCommand(); + + Command leftAutoAlignCommand(); } diff --git a/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java b/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java index 19a9a959..2af779f0 100644 --- a/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java @@ -23,7 +23,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentricFacingAngle; import com.google.auto.value.AutoBuilder; -import com.team2813.commands.RobotLocalization; import com.team2813.lib2813.limelight.BotPoseEstimate; import com.team2813.lib2813.preferences.PersistedConfiguration; import com.team2813.sysid.SwerveSysidRequest; @@ -573,4 +572,14 @@ public void close() { public Command resetPoseCommand() { return new InstantCommand(this::resetPose, this); } + + @Override + public Command rightAutoAlignCommand() { + return localization.getRightAutoAlignCommand(this::getPose); + } + + @Override + public Command leftAutoAlignCommand() { + return localization.getLeftAutoAlignCommand(this::getPose); + } } diff --git a/src/main/java/com/team2813/commands/RobotLocalization.java b/src/main/java/com/team2813/subsystems/drive/RobotLocalization.java similarity index 98% rename from src/main/java/com/team2813/commands/RobotLocalization.java rename to src/main/java/com/team2813/subsystems/drive/RobotLocalization.java index d847a253..3f75b69c 100644 --- a/src/main/java/com/team2813/commands/RobotLocalization.java +++ b/src/main/java/com/team2813/subsystems/drive/RobotLocalization.java @@ -1,4 +1,4 @@ -package com.team2813.commands; +package com.team2813.subsystems.drive; import static com.team2813.vision.VisionNetworkTables.HAS_DATA_TOPIC; import static com.team2813.vision.VisionNetworkTables.VISIBLE_APRIL_TAG_POSES_TOPIC; @@ -9,7 +9,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.Waypoint; import com.team2813.lib2813.preferences.PersistedConfiguration; -import com.team2813.subsystems.drive.Drive; import com.team2813.vision.VisionNetworkTables; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -33,7 +32,7 @@ * Make robot localization use photonvision, as now since I deleted the limelight impl. it is useless. */ -public class RobotLocalization { +class RobotLocalization { private static final Pose3d[] EMPTY_POSE3D_ARRAY = new Pose3d[0]; private final Configuration config; private final StructPublisher lastPosePublisher; From a2402949865a7253197a4c003a28b535eddfd8b4 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 21 Jun 2025 20:58:26 -0700 Subject: [PATCH 08/11] Remove Intake.asSubsystem() --- .../java/com/team2813/commands/OuttakeCommand.java | 6 +++++- .../com/team2813/subsystems/intake/Intake.java | 3 --- .../subsystems/intake/IntakeSubsystem.java | 14 -------------- 3 files changed, 5 insertions(+), 18 deletions(-) diff --git a/src/main/java/com/team2813/commands/OuttakeCommand.java b/src/main/java/com/team2813/commands/OuttakeCommand.java index 2ae09211..d57d8dd6 100644 --- a/src/main/java/com/team2813/commands/OuttakeCommand.java +++ b/src/main/java/com/team2813/commands/OuttakeCommand.java @@ -3,7 +3,11 @@ import com.team2813.subsystems.GroundIntake; import com.team2813.subsystems.GroundIntakePivot; import com.team2813.subsystems.intake.Intake; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; public final class OuttakeCommand { diff --git a/src/main/java/com/team2813/subsystems/intake/Intake.java b/src/main/java/com/team2813/subsystems/intake/Intake.java index 540c27d1..d1168be2 100644 --- a/src/main/java/com/team2813/subsystems/intake/Intake.java +++ b/src/main/java/com/team2813/subsystems/intake/Intake.java @@ -2,7 +2,6 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; public interface Intake extends AutoCloseable { @@ -22,8 +21,6 @@ static Intake create(NetworkTableInstance networkTableInstance) { Command stopMotorCommand(); - Subsystem asSubsystem(); - void stopMotor(); @Override diff --git a/src/main/java/com/team2813/subsystems/intake/IntakeSubsystem.java b/src/main/java/com/team2813/subsystems/intake/IntakeSubsystem.java index 9a785ca8..f12a7cf8 100644 --- a/src/main/java/com/team2813/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/com/team2813/subsystems/intake/IntakeSubsystem.java @@ -15,7 +15,6 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; /** This is the Intake. His name is Joe. Please be kind to him and say hi. Have a nice day! */ class IntakeSubsystem extends ParameterizedIntakeSubsystem implements Intake { @@ -53,24 +52,11 @@ void intakeCoral() { super.intakeGamePiece(); } - void outtakeCoral() { - super.outtakeGamePiece(); - } - - @Override - public Subsystem asSubsystem() { - return this; - } - @Override public Command bumpAlgaeCommand() { return setMotorDemandCommand(BUMP_VOLTAGE); } - void bumpAlgae() { - setMotorDemand(BUMP_VOLTAGE); - } - @Override public Command slowOuttakeItemCommand() { return setMotorDemandCommand(0.75 * PARAMS.outtakeDemand()); From f9b5c16b368e96c8c060e9debd490068f0d83980 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 28 Jun 2025 16:16:37 -0700 Subject: [PATCH 09/11] Cleanups to Elevator interface - Remove waitForSetpointCommand() and atPosition() (they were unused) - Remove redundant public modifiers --- .../subsystems/elevator/Elevator.java | 12 +--------- .../elevator/ElevatorSubsystem.java | 23 ------------------- 2 files changed, 1 insertion(+), 34 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/elevator/Elevator.java b/src/main/java/com/team2813/subsystems/elevator/Elevator.java index 45536837..254ca07a 100644 --- a/src/main/java/com/team2813/subsystems/elevator/Elevator.java +++ b/src/main/java/com/team2813/subsystems/elevator/Elevator.java @@ -36,19 +36,9 @@ static Elevator create(NetworkTableInstance networkTableInstance, DoubleSupplier */ Command setSetpointCommand(Position position); - /** - * Returns a command that waits for the elevator to reach the current setpoint. - * - *

If the elevator does not reach the setpoint in a reasonable period of time, the returned - * command will cancel itself. - */ - Command waitForSetpointCommand(); - Command disableCommand(); - boolean atPosition(); - - public enum Position implements Supplier { + enum Position implements Supplier { BOTTOM(-0.212500), TEST(10), TOP(16.358496); diff --git a/src/main/java/com/team2813/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/com/team2813/subsystems/elevator/ElevatorSubsystem.java index de9582b2..99137a2a 100644 --- a/src/main/java/com/team2813/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/com/team2813/subsystems/elevator/ElevatorSubsystem.java @@ -3,7 +3,6 @@ import static com.team2813.Constants.ELEVATOR_1; import static com.team2813.Constants.ELEVATOR_2; -import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.signals.NeutralModeValue; import com.team2813.commands.LockFunctionCommand; import com.team2813.lib2813.control.ControlMode; @@ -19,7 +18,6 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj2.command.*; -import java.util.Set; import java.util.function.DoubleSupplier; /** This is the Elevator. His name is Pablo. Please be kind to him and say hi. Have a nice day! */ @@ -27,7 +25,6 @@ class ElevatorSubsystem extends MotorSubsystem imple private static final Time MOVEMENT_TIMEOUT = Units.Seconds.of(2); private final BooleanPublisher atPosition; private final DoublePublisher position; - private double lastSetpointSetTime; // You see a message written in blood on the wall... // It reads: "THIS CODE WILL LIKELY HAVE TO BE *MAJORLY* REFACTORED @@ -62,31 +59,11 @@ public Command moveToPositionCommand(Position position) { .withTimeout(MOVEMENT_TIMEOUT); } - @Override - public Command waitForSetpointCommand() { - return new DeferredCommand( - () -> { - if (atPosition()) { - return Commands.none(); - } - double elapsedSecs = Utils.getCurrentTimeSeconds() - lastSetpointSetTime; - Time timeout = MOVEMENT_TIMEOUT.minus(Units.Seconds.of(elapsedSecs)); - return Commands.waitUntil(this::atPosition).withTimeout(timeout); - }, - Set.of(this)); - } - @Override public Command setSetpointCommand(Position position) { return new InstantCommand(() -> setSetpoint(position), this); } - @Override - public void setSetpoint(Position position) { - super.setSetpoint(position); - lastSetpointSetTime = Utils.getCurrentTimeSeconds(); - } - @Override public Command disableCommand() { return new InstantCommand(this::disable, this); From 27984682c80355ec70877b33b2459ed964195e46 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 21 Sep 2025 10:21:59 -0700 Subject: [PATCH 10/11] Add Drive.configurePathPlanner() This allows us to remove Drive.asSubsystem(). --- .../java/com/team2813/RobotContainer.java | 39 +------- .../com/team2813/subsystems/drive/Drive.java | 11 +-- .../subsystems/drive/DriveSubsystem.java | 94 +++++++++---------- 3 files changed, 48 insertions(+), 96 deletions(-) diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index a4d96340..c5fe4ec9 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -10,9 +10,6 @@ import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.events.EventTrigger; import com.team2813.commands.LockFunctionCommand; import com.team2813.commands.OuttakeCommand; @@ -37,14 +34,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.io.IOException; import java.util.ArrayList; import java.util.List; -import org.json.simple.parser.ParseException; public class RobotContainer implements AutoCloseable { - private static final DriverStation.Alliance ALLIANCE_USED_IN_PATHS = DriverStation.Alliance.Blue; - private final Climb climb; private final Intake intake; private final Elevator elevator; @@ -196,37 +189,7 @@ private static SendableChooser configureAuto( Intake intake, GroundIntake groundIntake, GroundIntakePivot groundIntakePivot) { - RobotConfig config; - try { - config = RobotConfig.fromGUISettings(); - } catch (IOException | ParseException e) { - // Or handle the error more gracefully - throw new RuntimeException("Could not get config!", e); - } - AutoBuilder.configure( - drive::getPose, // Robot pose supplier - drive - ::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - drive::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - drive::drive, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also - // optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following - // controller for holonomic drive trains - new PIDConstants(15, 0.0, 0), // Translation PID constants - new PIDConstants( - 6.85, 0.0, 1.3) // Rotation PID constants //make lower but 5 doesnt work - ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - return DriverStation.getAlliance() - .map(alliance -> alliance != ALLIANCE_USED_IN_PATHS) - .orElse(false); - }, - drive.asSubsystem() // Reference to this subsystem to set requirements - ); + drive.configurePathPlanner(); configureAutoCommands(elevator, intakePivot, intake, groundIntake, groundIntakePivot); return AutoBuilder.buildAutoChooser(); } diff --git a/src/main/java/com/team2813/subsystems/drive/Drive.java b/src/main/java/com/team2813/subsystems/drive/Drive.java index 650b22c6..01ea9b86 100644 --- a/src/main/java/com/team2813/subsystems/drive/Drive.java +++ b/src/main/java/com/team2813/subsystems/drive/Drive.java @@ -2,11 +2,9 @@ import com.team2813.sysid.SubsystemRegistry; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; public interface Drive extends AutoCloseable { @@ -23,17 +21,10 @@ static boolean onRed() { .orElse(false); } - Subsystem asSubsystem(); - - Pose2d getPose(); + void configurePathPlanner(); void setPose(Pose2d pose2d); - ChassisSpeeds getRobotRelativeSpeeds(); - - /** Handles autonomous driving. */ - void drive(ChassisSpeeds chassisSpeeds); - @Override void close(); diff --git a/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java b/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java index 2af779f0..7807e074 100644 --- a/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java @@ -21,8 +21,10 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import com.ctre.phoenix6.swerve.SwerveRequest.ApplyRobotSpeeds; import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; -import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentricFacingAngle; import com.google.auto.value.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.team2813.lib2813.limelight.BotPoseEstimate; import com.team2813.lib2813.preferences.PersistedConfiguration; import com.team2813.sysid.SwerveSysidRequest; @@ -38,17 +40,17 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.*; import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.io.IOException; import java.util.List; import java.util.function.DoubleSupplier; import java.util.stream.IntStream; +import org.json.simple.parser.ParseException; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonPoseEstimator; import org.photonvision.simulation.SimCameraProperties; @@ -57,6 +59,7 @@ /** This is the Drive. His name is Gary. Please be kind to him and say hi. Have a nice day! */ public class DriveSubsystem extends SubsystemBase implements Drive { + private static final DriverStation.Alliance ALLIANCE_USED_IN_PATHS = DriverStation.Alliance.Blue; private static final double DEFAULT_MAX_VELOCITY_METERS_PER_SECOND = 6; private static final double DEFAULT_MAX_ROTATIONS_PER_SECOND = 1.2; private static final Matrix PHOTON_MULTIPLE_TAG_STD_DEVS = @@ -342,11 +345,6 @@ default Builder maxVelocityInMetersPerSecond(double value) { } } - @Override - public Subsystem asSubsystem() { - return this; - } - private Rotation3d getRotation3d() { if (simDrivetrain != null) { return simDrivetrain.getRotation3d(); @@ -378,15 +376,6 @@ private double getPosition(int moduleId) { } private final ApplyRobotSpeeds applyRobotSpeedsApplier = new ApplyRobotSpeeds(); - /* - * IT IS ABSOLUTELY IMPERATIVE THAT YOU USE ApplyRobotSpeeds() RATHER THAN THE DEMENTED ApplyFieldSpeeds() HERE! - * If ApplyFieldSpeeds() is used, pathplanner & all autonomous paths will not function properly. - * This is because pathplanner knows where the robot is, but needs to use ApplyRobotSpeeds() in order to convert knowledge - * of where the robot is on the field, to instruction centered on the robot. - * Or something like this, I'm still not to sure how this works. - */ - private final FieldCentricFacingAngle fieldCentricFacingAngleApplier = - new FieldCentricFacingAngle(); private final FieldCentric fieldCentricApplier = new FieldCentric().withDriveRequestType(SwerveModule.DriveRequestType.Velocity); @@ -407,39 +396,12 @@ public void runSysIdRequest(SwerveSysidRequest request) { drivetrain.setControl(request); } - // Note: This is used for auto drive. - @Override - public void drive(ChassisSpeeds demand) { + /** Handles autonomous driving. */ + private void drive(ChassisSpeeds demand) { drivetrain.setControl(applyRobotSpeedsApplier.withSpeeds(demand)); } - public void turnToFace(Rotation2d rotation) { - drivetrain.setControl(fieldCentricFacingAngleApplier.withTargetDirection(rotation)); - } - - /** - * Sets the rotation velocity of the robot - * - * @param rotationRate rotation rate in radians per second - * @deprecated Unsafe; use {@link #setRotationVelocity(AngularVelocity)}, and specify the unit you - * are using - */ - @Deprecated - public void setRotationVelocityDouble(double rotationRate) { // Radians per second - drivetrain.setControl(fieldCentricApplier.withRotationalRate(rotationRate)); - } - - /** - * Sets the rotation velocity of the robot - * - * @param rotationRate rotation rate in units as defined by the WPIlib unit library. - */ - public void setRotationVelocity(AngularVelocity rotationRate) { - drivetrain.setControl(fieldCentricApplier.withRotationalRate(rotationRate)); - } - - @Override - public Pose2d getPose() { + private Pose2d getPose() { return drivetrain.getState().Pose; } @@ -474,7 +436,43 @@ public void setPose(Pose2d pose) { } @Override - public ChassisSpeeds getRobotRelativeSpeeds() { + public void configurePathPlanner() { + RobotConfig config = loadRobotConfig(); + com.pathplanner.lib.auto.AutoBuilder.configure( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::drive, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also + // optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following + // controller for holonomic drive trains + new PIDConstants(15, 0.0, 0), // Translation PID constants + new PIDConstants( + 6.85, 0.0, 1.3) // Rotation PID constants //make lower but 5 doesnt work + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + return DriverStation.getAlliance() + .map(alliance -> alliance != ALLIANCE_USED_IN_PATHS) + .orElse(false); + }, + this // Reference to this subsystem to set requirements + ); + } + + private RobotConfig loadRobotConfig() { + try { + return RobotConfig.fromGUISettings(); + } catch (IOException | ParseException e) { + // Or handle the error more gracefully + throw new RuntimeException("Could not get config!", e); + } + } + + private ChassisSpeeds getRobotRelativeSpeeds() { return this.drivetrain.getKinematics().toChassisSpeeds(this.drivetrain.getState().ModuleStates); } From a7fb555658439bcc07e94fbc813b311af2252d08 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 21 Sep 2025 10:25:38 -0700 Subject: [PATCH 11/11] Remove dependency of DriveSubsystem on Localization --- .../com/team2813/subsystems/drive/Drive.java | 7 +----- .../subsystems/drive/DriveSubsystem.java | 22 +++---------------- 2 files changed, 4 insertions(+), 25 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/drive/Drive.java b/src/main/java/com/team2813/subsystems/drive/Drive.java index 01ea9b86..cdefb109 100644 --- a/src/main/java/com/team2813/subsystems/drive/Drive.java +++ b/src/main/java/com/team2813/subsystems/drive/Drive.java @@ -9,8 +9,7 @@ public interface Drive extends AutoCloseable { static Drive create(NetworkTableInstance networkTableInstance, SubsystemRegistry registry) { - RobotLocalization localization = new RobotLocalization(networkTableInstance); - DriveSubsystem drive = new DriveSubsystem(networkTableInstance, localization); + DriveSubsystem drive = new DriveSubsystem(networkTableInstance); registry.addSubsystem(drive); return drive; } @@ -31,8 +30,4 @@ static boolean onRed() { Command enableSlowModeCommand(boolean enable); Command resetPoseCommand(); - - Command rightAutoAlignCommand(); - - Command leftAutoAlignCommand(); } diff --git a/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java b/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java index 7807e074..b1ed7bbe 100644 --- a/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java @@ -64,7 +64,6 @@ public class DriveSubsystem extends SubsystemBase implements Drive { private static final double DEFAULT_MAX_ROTATIONS_PER_SECOND = 1.2; private static final Matrix PHOTON_MULTIPLE_TAG_STD_DEVS = new Matrix<>(Nat.N3(), Nat.N1(), new double[] {0.1, 0.1, 0.1}); - private final RobotLocalization localization; private final SwerveDrivetrain drivetrain; private final SimulatedSwerveDrivetrain simDrivetrain; private final VisionSystemSim simVisionSystem; @@ -185,16 +184,11 @@ default Builder maxVelocityInMetersPerSecond(double value) { } } - DriveSubsystem(NetworkTableInstance networkTableInstance, RobotLocalization localization) { - this(networkTableInstance, localization, Configuration.fromPreferences()); + DriveSubsystem(NetworkTableInstance networkTableInstance) { + this(networkTableInstance, Configuration.fromPreferences()); } - DriveSubsystem( - NetworkTableInstance networkTableInstance, - RobotLocalization localization, - Configuration config) { - this.localization = localization; - + DriveSubsystem(NetworkTableInstance networkTableInstance, Configuration config) { var aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); photonPoseEstimator = new MultiPhotonPoseEstimator.Builder( @@ -570,14 +564,4 @@ public void close() { public Command resetPoseCommand() { return new InstantCommand(this::resetPose, this); } - - @Override - public Command rightAutoAlignCommand() { - return localization.getRightAutoAlignCommand(this::getPose); - } - - @Override - public Command leftAutoAlignCommand() { - return localization.getLeftAutoAlignCommand(this::getPose); - } }