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 b8e1cb1d..c5fe4ec9 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -10,14 +10,18 @@ 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.*; +import com.team2813.commands.LockFunctionCommand; +import com.team2813.commands.OuttakeCommand; 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.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; import com.team2813.sysid.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -30,15 +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 java.util.Set; -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; @@ -51,20 +50,19 @@ public class RobotContainer implements AutoCloseable { private final SysIdRoutineSelector sysIdRoutineSelector; public RobotContainer(ShuffleboardTabs shuffleboard, NetworkTableInstance networkTableInstance) { - var localization = new RobotLocalization(networkTableInstance); - this.drive = new Drive(networkTableInstance, localization); - this.elevator = new Elevator(networkTableInstance); - this.intakePivot = new IntakePivot(networkTableInstance); - this.climb = new Climb(networkTableInstance); - this.intake = new Intake(networkTableInstance); + var registry = new SubsystemRegistry(); + this.drive = Drive.create(networkTableInstance, registry); + this.elevator = Elevator.create(networkTableInstance, () -> -OPERATOR_CONTROLLER.getRightY()); + this.intakePivot = IntakePivot.create(networkTableInstance); + this.climb = Climb.create(networkTableInstance); + this.intake = Intake.create(networkTableInstance); this.groundIntakePivot = new GroundIntakePivot(networkTableInstance); autoChooser = configureAuto(drive, elevator, intakePivot, intake, groundIntake, groundIntakePivot); SmartDashboard.putData("Auto Routine", autoChooser); sysIdRoutineSelector = - new SysIdRoutineSelector( - new SubsystemRegistry(Set.of(drive)), RobotContainer::getSysIdRoutines, shuffleboard); - configureBindings(localization); + new SysIdRoutineSelector(registry, RobotContainer::getSysIdRoutines, shuffleboard); + configureBindings(); } /** @@ -87,43 +85,30 @@ private static void configureAutoCommands( NamedCommands.registerCommand( "PrepareL2", new ParallelCommandGroup( - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), - new InstantCommand(() -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator))); + intakePivot.setSetpointCommand(IntakePivot.Rotations.OUTTAKE), + 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))); + 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( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), - elevator) - .withTimeout(SECONDS_2), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), - intakePivot) - .withTimeout(SECONDS_2)), + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.OUTTAKE)), intake.slowOuttakeItemCommand(), new WaitCommand(DROP_CORAL), new ParallelCommandGroup( intake.stopMotorCommand(), - new InstantCommand(elevator::disable, elevator), - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); + elevator.disableCommand(), + intakePivot.setSetpointCommand(IntakePivot.Rotations.INTAKE)))); NamedCommands.registerCommand( "ScoreL1", @@ -136,102 +121,65 @@ private static void configureAutoCommands( NamedCommands.registerCommand( "ScoreL3", new SequentialCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.TOP), - elevator) - .withTimeout(SECONDS_2), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), - intakePivot) - .withTimeout(SECONDS_2), + elevator.moveToPositionCommand(Elevator.Position.TOP), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.OUTTAKE), intake.slowOuttakeItemCommand(), new WaitCommand(DROP_CORAL), new ParallelCommandGroup( intake.stopMotorCommand(), - new InstantCommand(() -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator), - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); + elevator.setSetpointCommand(Elevator.Position.BOTTOM), + intakePivot.setSetpointCommand(IntakePivot.Rotations.INTAKE)))); NamedCommands.registerCommand( "BumpAlgaeLow", new SequentialCommandGroup( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), - elevator) - .withTimeout(SECONDS_2), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), - intakePivot) - .withTimeout(SECONDS_2)), + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.ALGAE_BUMP)), intake.outtakeItemCommand(), new WaitCommand(SECONDS_1), // TODO: Wait until we bump low algae new ParallelCommandGroup( intake.stopMotorCommand(), - new InstantCommand(() -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator), - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); + elevator.setSetpointCommand(Elevator.Position.BOTTOM), + intakePivot.setSetpointCommand(IntakePivot.Rotations.INTAKE)))); NamedCommands.registerCommand( "BumpAlgaeHigh", new SequentialCommandGroup( new ParallelCommandGroup( new SequentialCommandGroup( - new WaitCommand(0.05), - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.TOP), - elevator) - .withTimeout(SECONDS_2)), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), - intakePivot) - .withTimeout(SECONDS_2)), + new WaitCommand(0.05), elevator.setSetpointCommand(Elevator.Position.TOP)), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.ALGAE_BUMP)), intake.bumpAlgaeCommand(), new WaitCommand(SECONDS_1), // TODO: Wait until we bump high algae new ParallelCommandGroup( intake.stopMotorCommand(), - new InstantCommand(() -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator), - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), intakePivot)))); + elevator.setSetpointCommand(Elevator.Position.BOTTOM), + intakePivot.setSetpointCommand(IntakePivot.Rotations.INTAKE)))); NamedCommands.registerCommand( "IntakeCoral", new SequentialCommandGroup( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), - elevator) - .withTimeout(SECONDS_2), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), - intakePivot) - .withTimeout(SECONDS_2)), + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.INTAKE)), intake.intakeItemCommand(), new WaitUntilCommand(intake::hasCoral).withTimeout(INTAKE_TIME), new ParallelCommandGroup( intake.intakeItemCommand(), - new InstantCommand(elevator::disable, elevator), - new InstantCommand(intakePivot::disable, intakePivot)))); + elevator.disableCommand(), + intakePivot.disableCommand()))); new EventTrigger("PrepareL2") .onTrue( new ParallelCommandGroup( - new InstantCommand( - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), intakePivot), - new InstantCommand( - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), elevator))); + intakePivot.setSetpointCommand(IntakePivot.Rotations.OUTTAKE), + elevator.setSetpointCommand(Elevator.Position.BOTTOM))); new EventTrigger("PrepareL3") .onTrue( - new DeferredCommand( - () -> NamedCommands.getCommand("PrepareL3"), Set.of(intakePivot, elevator))); + new ParallelCommandGroup( + intakePivot.setSetpointCommand(IntakePivot.Rotations.OUTTAKE), + elevator.setSetpointCommand(Elevator.Position.TOP))); } private static SendableChooser configureAuto( @@ -241,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 // Reference to this subsystem to set requirements - ); + drive.configurePathPlanner(); configureAutoCommands(elevator, intakePivot, intake, groundIntake, groundIntakePivot); return AutoBuilder.buildAutoChooser(); } @@ -292,10 +210,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", @@ -305,10 +223,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)", @@ -321,19 +239,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) { + private void configureBindings() { // 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( () -> @@ -343,7 +260,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 @@ -368,17 +285,9 @@ 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)), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.INTAKE), - intakePivot) - .withTimeout(Units.Seconds.of(0.5))), - intake.intakeItemCommand())); + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.INTAKE), + intake.intakeItemCommand()))); INTAKE_BUTTON.onFalse(intake.stopMotorCommand()); GROUND_CORAL_INTAKE.whileTrue( @@ -411,58 +320,33 @@ private void configureBindings(RobotLocalization localization) { PREP_L2_CORAL.onTrue( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, - () -> elevator.setSetpoint(Elevator.Position.BOTTOM), - elevator), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), - intakePivot))); + elevator.moveToPositionCommand(Elevator.Position.BOTTOM), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.OUTTAKE))); PREP_L3_CORAL.onTrue( new ParallelCommandGroup( - new LockFunctionCommand( - elevator::atPosition, () -> elevator.setSetpoint(Elevator.Position.TOP), elevator), - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.OUTTAKE), - intakePivot))); + elevator.moveToPositionCommand(Elevator.Position.TOP), + intakePivot.moveToPositionCommand(IntakePivot.Rotations.OUTTAKE))); /* Is there code for algea intake? R2.whileTrue( new InstantCommand() );*/ - elevator.setDefaultCommand( - new ElevatorDefaultCommand(elevator, () -> -OPERATOR_CONTROLLER.getRightY())); - intakePivot.setDefaultCommand( - new ManuelIntakePivot(intakePivot, () -> -OPERATOR_CONTROLLER.getLeftY())); CLIMB_DOWN.onTrue( new SequentialCommandGroup( - new LockFunctionCommand( - intakePivot::atPosition, - () -> intakePivot.setSetpoint(IntakePivot.Rotations.ALGAE_BUMP), - intakePivot), - new InstantCommand(climb::lower, climb))); - CLIMB_DOWN.onFalse(new InstantCommand(climb::stop, climb)); + intakePivot.moveToPositionCommand(IntakePivot.Rotations.ALGAE_BUMP), + 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( - 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/commands/OuttakeCommand.java b/src/main/java/com/team2813/commands/OuttakeCommand.java index b794404b..d57d8dd6 100644 --- a/src/main/java/com/team2813/commands/OuttakeCommand.java +++ b/src/main/java/com/team2813/commands/OuttakeCommand.java @@ -2,8 +2,12 @@ import com.team2813.subsystems.GroundIntake; import com.team2813.subsystems.GroundIntakePivot; -import com.team2813.subsystems.Intake; -import edu.wpi.first.wpilibj2.command.*; +import com.team2813.subsystems.intake.Intake; +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/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/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/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..cdefb109 --- /dev/null +++ b/src/main/java/com/team2813/subsystems/drive/Drive.java @@ -0,0 +1,33 @@ +package com.team2813.subsystems.drive; + +import com.team2813.sysid.SubsystemRegistry; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; + +public interface Drive extends AutoCloseable { + + static Drive create(NetworkTableInstance networkTableInstance, SubsystemRegistry registry) { + DriveSubsystem drive = new DriveSubsystem(networkTableInstance); + registry.addSubsystem(drive); + return drive; + } + + static boolean onRed() { + return DriverStation.getAlliance() + .map(alliance -> alliance == DriverStation.Alliance.Red) + .orElse(false); + } + + void configurePathPlanner(); + + void setPose(Pose2d pose2d); + + @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 86% rename from src/main/java/com/team2813/subsystems/Drive.java rename to src/main/java/com/team2813/subsystems/drive/DriveSubsystem.java index d5647f9b..b1ed7bbe 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; @@ -21,10 +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.team2813.commands.DefaultDriveCommand; -import com.team2813.commands.RobotLocalization; +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; @@ -40,15 +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.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; @@ -56,12 +58,12 @@ 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 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 = 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; @@ -141,7 +143,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,16 +184,11 @@ default Builder maxVelocityInMetersPerSecond(double value) { } } - public Drive(NetworkTableInstance networkTableInstance, RobotLocalization localization) { - this(networkTableInstance, localization, Configuration.fromPreferences()); + DriveSubsystem(NetworkTableInstance networkTableInstance) { + this(networkTableInstance, Configuration.fromPreferences()); } - public Drive( - NetworkTableInstance networkTableInstance, - RobotLocalization localization, - Configuration config) { - this.localization = localization; - + DriveSubsystem(NetworkTableInstance networkTableInstance, Configuration config) { var aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); photonPoseEstimator = new MultiPhotonPoseEstimator.Builder( @@ -350,7 +347,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(), @@ -373,29 +370,14 @@ 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); - 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) @@ -408,37 +390,12 @@ public void runSysIdRequest(SwerveSysidRequest request) { drivetrain.setControl(request); } - // Note: This is used for auto drive. - 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)); - } - - public Pose2d getPose() { + private Pose2d getPose() { return drivetrain.getState().Pose; } @@ -454,6 +411,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,7 +429,44 @@ public void setPose(Pose2d pose) { } } - public ChassisSpeeds getRobotRelativeSpeeds() { + @Override + 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); } @@ -554,8 +549,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 +559,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/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 59c11b58..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; 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; 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/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..254ca07a --- /dev/null +++ b/src/main/java/com/team2813/subsystems/elevator/Elevator.java @@ -0,0 +1,57 @@ +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(NetworkTableInstance networkTableInstance, DoubleSupplier movement) { + return new ElevatorSubsystem(networkTableInstance, 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); + + Command disableCommand(); + + 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 70% rename from src/main/java/com/team2813/subsystems/Elevator.java rename to src/main/java/com/team2813/subsystems/elevator/ElevatorSubsystem.java index c410f48e..99137a2a 100644 --- a/src/main/java/com/team2813/subsystems/Elevator.java +++ b/src/main/java/com/team2813/subsystems/elevator/ElevatorSubsystem.java @@ -1,10 +1,10 @@ -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.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 +15,16 @@ 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.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; // You see a message written in blood on the wall... // It reads: "THIS CODE WILL LIKELY HAVE TO BE *MAJORLY* REFACTORED @@ -28,7 +32,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 +42,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 +54,30 @@ 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 setSetpointCommand(Position position) { + return new InstantCommand(() -> setSetpoint(position), this); } - 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 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() { 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..d1168be2 --- /dev/null +++ b/src/main/java/com/team2813/subsystems/intake/Intake.java @@ -0,0 +1,28 @@ +package com.team2813.subsystems.intake; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.Command; + +public interface Intake extends AutoCloseable { + + static Intake create(NetworkTableInstance networkTableInstance) { + return new IntakeSubsystem(networkTableInstance); + } + + boolean hasCoral(); + + Command bumpAlgaeCommand(); + + Command intakeItemCommand(); + + Command outtakeItemCommand(); + + Command slowOuttakeItemCommand(); + + Command stopMotorCommand(); + + void stopMotor(); + + @Override + void close(); +} 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/subsystems/Intake.java b/src/main/java/com/team2813/subsystems/intake/IntakeSubsystem.java similarity index 82% rename from src/main/java/com/team2813/subsystems/Intake.java rename to src/main/java/com/team2813/subsystems/intake/IntakeSubsystem.java index 7b14dc18..f12a7cf8 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,6 +9,7 @@ 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; @@ -16,21 +17,22 @@ import edu.wpi.first.wpilibj2.command.Command; /** 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 +48,21 @@ public Intake(NetworkTableInstance networkTableInstance) { hasCoralPublisher = networkTable.getBooleanTopic("Has Coral").publish(); } + void intakeCoral() { + super.intakeGamePiece(); + } + + @Override public Command bumpAlgaeCommand() { return setMotorDemandCommand(BUMP_VOLTAGE); } + @Override public Command slowOuttakeItemCommand() { return setMotorDemandCommand(0.75 * PARAMS.outtakeDemand()); } + @Override public boolean hasCoral() { return !beamBreak.get(); } 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 } } 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()); } } 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..8e31d99c --- /dev/null +++ b/src/test/java/com/team2813/subsystems/intake/IntakeSubsystemTest.java @@ -0,0 +1,132 @@ +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 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 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, 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, 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); + + CommandTesterInterface.runUntilComplete(command); + + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + } + } + + @Test + 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, CommandTester CommandTesterInterface) { + try (var intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + intake.intakeCoral(); + Command command = intake.outtakeItemCommand(); + CommandTesterInterface.runUntilComplete(command); + command = intake.stopMotorCommand(); + + 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, 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, CommandTester CommandTesterInterface) { + try (var intake = new IntakeSubsystem(fakeMotor, mockBeamBreak, ntInstance)) { + Command command = intake.bumpAlgaeCommand(); + CommandTesterInterface.runUntilComplete(command); + command = intake.stopMotorCommand(); + + CommandTesterInterface.runUntilComplete(command); + + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0.0); + assertThat(fakeMotor.dutyCycle).isWithin(0.01).of(0); + } + } +}