Protect state machine from improper use

src/main/java/frc/robot/subsystems/StateMachine.java

@@ -1,29 +1,33 @@
 package frc.robot.subsystems;
 
-import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class StateMachine extends SubsystemBase {
+  private SystemState m_currentState;
 
-public interface StateMachine extends Subsystem {
   /**
-   * Get current system state
-   * @return Current system state
+   * Create a state machine
+   * @param initialState Starting state for state machine
    */
-  public SystemState getState();
+  public StateMachine(SystemState initialState) {
+    this.m_currentState = initialState;
+    setDefaultCommand(new StateCommand(this::getState, this).repeatedly());
+  }
 
   /**
    * Iterate system to next state
    * <p>
    * ONLY TO BE USED BY {@link StateCommand#end(boolean)}
-   * <p>
-   * DO NOT CALL THIS METHOD!
    */
-  public void setState(SystemState state);
+  void setState(SystemState state) {
+    m_currentState = state;
+  }
 
   /**
-   * Sets the default command for this subsystem using the state machine architecture
-   * @param commands List of state commands
+   * Get current system state
+   * @return Current system state
    */
-  public default void setDefaultCommand() {
-    // Set default command to a repeating select command, with the state getter as the selector
-    setDefaultCommand(new StateCommand(() -> getState(), this).repeatedly());
+  public SystemState getState() {
+    return m_currentState;
   }
 }

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -14,13 +14,12 @@
 import edu.wpi.first.units.Dimensionless;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Units;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants;
 import frc.robot.subsystems.StateMachine;
 import frc.robot.subsystems.SystemState;
 
-public class IntakeSubsystem extends SubsystemBase implements StateMachine, AutoCloseable {
+public class IntakeSubsystem extends StateMachine implements AutoCloseable {
   public static class Hardware {
     private Spark rollerMotor;
 
@@ -74,25 +73,21 @@ public State nextState() {
   private static Trigger s_intakeButton = new Trigger(() -> false);
   private static Trigger s_outtakeButton = new Trigger(() -> false);
 
-  private SystemState m_currentState;
-
   private final Measure<Dimensionless> m_rollerVelocity;
 
   private final Spark m_rollerMotor;
 
   /** Creates a new IntakeSubsystem. */
   private IntakeSubsystem(Hardware intakeHardware, Measure<Dimensionless> rollerVelocity) {
+    super(State.IDLE);
     this.m_rollerMotor = intakeHardware.rollerMotor;
     this.m_rollerVelocity = rollerVelocity;
-    this.m_currentState = State.IDLE;
 
     // Reset motor to defaults
     m_rollerMotor.restoreFactoryDefaults();
 
     // Set idle mode
     m_rollerMotor.setIdleMode(IdleMode.kCoast);
-
-    setDefaultCommand();
   }
 
   /**
@@ -143,19 +138,7 @@ private void stop() {
 
   @Override
   public void periodic() {
-    Logger.recordOutput(getName() + "/State", m_currentState.toString());
-  }
-
-
-  @Override
-  public SystemState getState() {
-    return m_currentState;
-  }
-
-
-  @Override
-  public void setState(SystemState state) {
-    m_currentState = state;
+    Logger.recordOutput(getName() + "/State", getState().toString());
   }
 
   /**