WIP: Create interfaces for subsystems

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() {

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 {
 

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;
 

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();
+}

src/main/java/com/team2813/subsystems/Climb.java → src/main/java/com/team2813/subsystems/climb/ClimbSubsystem.java

@@ -1,11 +1,12 @@
-package com.team2813.subsystems;
+package com.team2813.subsystems.climb;
 
 import static com.team2813.Constants.CLIMB_1;
 import static com.team2813.Constants.CLIMB_2;
 
 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);
   }
 

src/main/java/com/team2813/commands/DefaultDriveCommand.java → 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<Double> xSupplier;
   private final Supplier<Double> ySupplier;
   private final Supplier<Double> rotationSupplier;
 
-  public DefaultDriveCommand(
-      Drive drive,
+  public DefaultCommand(
+      DriveSubsystem drive,
       Supplier<Double> xSupplier,
       Supplier<Double> ySupplier,
       Supplier<Double> rotationSupplier) {

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();
+}