Enhance Indexer and Intake subsystems in simulation

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

@@ -2,6 +2,9 @@
 
 import frc.robot.GlobalConstants;
 import frc.robot.pioneersLib.subsystem.Subsystem;
+import frc.robot.subsystems.Indexer.IndexerManager.IndexerInterface;
+import frc.robot.subsystems.Indexer.IndexerManager.IndexerInterfaceStates;
+import frc.robot.subsystems.Intake.Intake;
 import org.littletonrobotics.junction.Logger;
 
 public class Indexer extends Subsystem<IndexerStates> {
@@ -10,20 +13,61 @@ public class Indexer extends Subsystem<IndexerStates> {
 	private IndexerIO io;
 	private IndexerIOInputsAutoLogged inputs;
 
+	// This is a rogue subsystem 🥶🥶🥶, ngl if we were power scaling subsystems this would be comp for drive in terms of aura
 	private Indexer() {
 		super("Indexer", IndexerStates.OFF);
-		addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () ->
-			io.nextSensorTriggered()
-		);
-		addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> true); //TODO: actually get intake beam break
-
 		this.io = switch (GlobalConstants.ROBOT_MODE) {
 			case SIM -> new IndexerIOSim();
 			case REAL -> new IndexerIOTalonFX();
 			case TESTING -> new IndexerIOTalonFX();
 			case REPLAY -> new IndexerIOSim();
 		};
 		this.inputs = new IndexerIOInputsAutoLogged();
+
+		// State for scoring
+		addTrigger(
+			IndexerStates.OFF,
+			IndexerStates.SCORING,
+			() -> IndexerInterface.getInstance().getState() == IndexerInterfaceStates.SCORING
+		);
+
+		// Return to Off
+		addTrigger(
+			IndexerStates.SCORING,
+			IndexerStates.OFF,
+			() -> IndexerInterface.getInstance().getState() == IndexerInterfaceStates.OFF
+		);
+		addTrigger(
+			IndexerStates.AUTO_OFF,
+			IndexerStates.OFF,
+			() -> IndexerInterface.getInstance().getState() == IndexerInterfaceStates.OFF
+		);
+		addTrigger(
+			IndexerStates.AUTO_ON,
+			IndexerStates.OFF,
+			() -> IndexerInterface.getInstance().getState() == IndexerInterfaceStates.OFF
+		);
+
+		// Automaticall indexing
+		addTrigger(
+			IndexerStates.OFF,
+			IndexerStates.AUTO_OFF,
+			() -> IndexerInterface.getInstance().getState() == IndexerInterfaceStates.AUTONOMOUS
+		);
+		addTrigger(
+			IndexerStates.AUTO_OFF,
+			IndexerStates.AUTO_ON,
+			() ->
+				IndexerInterface.getInstance().getState() == IndexerInterfaceStates.AUTONOMOUS &&
+				Intake.getInstance().hasGamepiece()
+		);
+		addTrigger(
+			IndexerStates.AUTO_ON,
+			IndexerStates.AUTO_OFF,
+			() ->
+				IndexerInterface.getInstance().getState() != IndexerInterfaceStates.AUTONOMOUS &&
+				io.nextSensorTriggered()
+		);
 	}
 
 	public static Indexer getInstance() {
@@ -45,8 +89,6 @@ public void stop() {
 	public void periodic() {
 		super.periodic();
 
-		//TODO: Add output managing
-
 		Logger.processInputs("Indexer", inputs);
 		io.updateInputs(inputs);
 	}
@@ -55,7 +97,11 @@ public int getNumberOfPieces() {
 		return io.getNumberOfPieces();
 	}
 
-	public boolean isEmpty() {
-		return io.getNumberOfPieces() == 0;
+	public boolean nextSensorTriggered() {
+		return io.nextSensorTriggered();
+	}
+
+	public double getStateTime() {
+		return getStateTime();
 	}
 }

src/main/java/frc/robot/subsystems/Indexer/IndexerConstants.java

@@ -1,5 +1,9 @@
 package frc.robot.subsystems.Indexer;
 
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.units.measure.Time;
+
 public final class IndexerConstants {
 
 	public static final int SPINNER_ID = 1;
@@ -16,6 +20,8 @@ public final class IndexerConstants {
 
 	public static final int MAX_GAME_PIECES = 6;
 
+	public static final Time SIMULATED_INDEXING_TIME = Seconds.of(1);
+
 	public static final double ON = 5;
 	public static final double OFF = 0;
 }

src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java

@@ -1,5 +1,7 @@
 package frc.robot.subsystems.Indexer;
 
+import static edu.wpi.first.units.Units.Seconds;
+
 import com.pathplanner.lib.config.PIDConstants;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -62,7 +64,11 @@ public void configurePID(PIDConstants constants) {
 		speedController = new PIDController(constants.kP, constants.kI, constants.kD);
 	}
 
-	public boolean[] getBeamBreakArray() {
-		return beamBreakArray;
+	@Override
+	public boolean nextSensorTriggered() {
+		return (
+			Indexer.getInstance().getStateTime() >
+			IndexerConstants.SIMULATED_INDEXING_TIME.in(Seconds)
+		);
 	}
 }

src/main/java/frc/robot/subsystems/Indexer/IndexerIOTalonFX.java

@@ -48,7 +48,7 @@ public void setSetpoint(double setpoint) {
 		);
 		wheelSpeedpoint = setpoint;
 
-		wheelMotor.setVoltage(wheelAppliedVoltage);
+		wheelMotor.set(wheelAppliedVoltage);
 	}
 
 	@Override

src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterface.java

@@ -0,0 +1,36 @@
+package frc.robot.subsystems.Indexer.IndexerManager;
+
+import frc.robot.pioneersLib.subsystem.Subsystem;
+import frc.robot.subsystems.Indexer.Indexer;
+import frc.robot.subsystems.Indexer.IndexerConstants;
+
+public class IndexerInterface extends Subsystem<IndexerInterfaceStates> {
+
+	private static IndexerInterface instance;
+
+	private IndexerInterface() {
+		super("Indexer Manager", IndexerInterfaceStates.OFF);
+	}
+
+	public static IndexerInterface getInstance() {
+		if (instance == null) {
+			instance = new IndexerInterface();
+		}
+		return instance;
+	}
+
+	public int getNumberOfPieces() {
+		return Indexer.getInstance().getNumberOfPieces();
+	}
+
+	public boolean isEmpty() {
+		return Indexer.getInstance().getNumberOfPieces() == 0;
+	}
+
+	public boolean isFull() {
+		return Indexer.getInstance().getNumberOfPieces() >= IndexerConstants.MAX_GAME_PIECES;
+	}
+
+	@Override
+	public void runState() {}
+}

src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterfaceStates.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.Indexer.IndexerManager;
+
+import frc.robot.pioneersLib.subsystem.SubsystemStates;
+import frc.robot.subsystems.Indexer.IndexerStates;
+
+public enum IndexerInterfaceStates implements SubsystemStates {
+	SCORING("SCORING"),
+	OFF("OFF"),
+	AUTONOMOUS("Autonomous");
+
+	IndexerInterfaceStates(String stateString) {
+		this.stateString = stateString;
+	}
+
+	private String stateString;
+
+	@Override
+	public String getStateString() {
+		return stateString;
+	}
+}

src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java

@@ -4,8 +4,8 @@
 
 public enum IndexerStates implements SubsystemStates {
 	OFF("Off", IndexerConstants.OFF),
-	AUTONOMOUS_ON("Autonomous On", IndexerConstants.ON),
-	AUTONOMOUS_OFF("Autonomous Off", IndexerConstants.OFF),
+	AUTO_ON("Indexing", IndexerConstants.ON),
+	AUTO_OFF("Indexing", IndexerConstants.OFF),
 	SCORING("Scoring", IndexerConstants.ON);
 
 	private String stateString;

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

@@ -7,8 +7,8 @@
 
 public class Intake extends Subsystem<IntakeStates> {
 
-	IntakeIO io;
-	IntakeIOInputsAutoLogged inputs;
+	private IntakeIO io;
+	private IntakeIOInputsAutoLogged inputs;
 	private static Intake instance;
 
 	private Intake(IntakeIO io) {
@@ -39,4 +39,12 @@ public static Intake getInstance() {
 		}
 		return instance;
 	}
+
+	public boolean hasGamepiece() {
+		return io.hasGamepiece();
+	}
+
+	public double getStateTime() {
+		return getStateTime();
+	}
 }

src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java

@@ -4,12 +4,14 @@
 import static edu.wpi.first.units.Units.KilogramSquareMeters;
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
 
 import com.pathplanner.lib.config.PIDConstants;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.MomentOfInertia;
+import edu.wpi.first.units.measure.Time;
 
 public final class IntakeConstants {
 
@@ -38,12 +40,15 @@ public static final class Sim {
 		public static final MomentOfInertia WHEEL_MOTOR_MOI = KilogramSquareMeters.of(1); // random value
 		public static final double WHEEL_MOTOR_GEARING = 3;
 		public static final PIDConstants WHEEL_PID_CONSTANTS = new PIDConstants(0.0012, 0, 0);
+
+		public static final Time SIMULATED_INTAKING_TIME = Seconds.of(1);
 	}
 
 	public static final class Real {
 
 		public static final int WHEEL_MOTOR_CANID = 10;
 		public static final int PIVOT_MOTOR_CANID = 11;
+		public static final int BEAM_BREAK_DIO_PORT = 10;
 		public static final PIDConstants PIVOT_PID_CONSTANTS = new PIDConstants(3, 0, 0);
 		public static final PIDConstants WHEEL_PID_CONSTANTS = new PIDConstants(0.0012, 0, 0);
 	}

src/main/java/frc/robot/subsystems/Intake/IntakeIO.java

@@ -15,9 +15,13 @@ public static class IntakeIOInputs {
 		public double wheelSpeedSetpoint;
 	}
 
-	public void updateInputs(IntakeIOInputs input);
+	public default void updateInputs(IntakeIOInputs input) {}
 
-	public void setPivotSetpoint(double pivotSetpoint);
+	public default void setPivotSetpoint(double pivotSetpoint) {}
 
-	public void setWheelSpeed(double wheelSpeed);
+	public default void setWheelSpeed(double wheelSpeed) {}
+
+	public default boolean hasGamepiece() {
+		return false;
+	}
 }

src/main/java/frc/robot/subsystems/Intake/IntakeIOReal.java

@@ -3,6 +3,7 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.GlobalConstants;
 import frc.robot.GlobalConstants.RobotMode;
@@ -15,6 +16,7 @@ public class IntakeIOReal implements IntakeIO {
 	private PIDController wheelSpeedController;
 	private double pivotPositionSetpoint;
 	private double wheelSpeedSetpoint;
+	private DigitalInput beamBreak;
 
 	public IntakeIOReal() {
 		wheelMotor = new TalonFX(IntakeConstants.Real.WHEEL_MOTOR_CANID);
@@ -31,6 +33,7 @@ public IntakeIOReal() {
 			IntakeConstants.Real.WHEEL_PID_CONSTANTS.kI,
 			IntakeConstants.Real.WHEEL_PID_CONSTANTS.kD
 		);
+		beamBreak = new DigitalInput(IntakeConstants.Real.BEAM_BREAK_DIO_PORT);
 	}
 
 	@Override
@@ -67,4 +70,9 @@ public void setWheelSpeed(double wheelSpeed) {
 		);
 		wheelMotor.setVoltage(voltage);
 	}
+
+	@Override
+	public boolean hasGamepiece() {
+		return beamBreak.get();
+	}
 }

src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems.Intake;
 
+import static edu.wpi.first.units.Units.Seconds;
 import static frc.robot.GlobalConstants.SIM_DELTA_TIME;
 
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -26,6 +27,8 @@ public class IntakeIOSim implements IntakeIO {
 	private double wheelSpeedSetpoint;
 	private double pivotPositionSetpoint;
 
+	private double lastIntookSimulatedGampieceTime;
+
 	IntakeIOSim() {
 		wheelMotor = new TalonFX(IntakeConstants.Real.WHEEL_MOTOR_CANID);
 		pivotMotor = new TalonFX(IntakeConstants.Real.PIVOT_MOTOR_CANID);
@@ -65,6 +68,7 @@ public class IntakeIOSim implements IntakeIO {
 
 		wheelSpeedSetpoint = 0;
 		pivotPositionSetpoint = 0;
+		lastIntookSimulatedGampieceTime = 0;
 	}
 
 	@Override
@@ -102,4 +106,14 @@ public void setWheelSpeed(double wheelSpeedSetpoint) {
 			)
 		);
 	}
+
+	public boolean hasGamepiece() {
+		boolean gamepieceInIntake =
+			Intake.getInstance().getStateTime() - lastIntookSimulatedGampieceTime >
+			IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds);
+		if (gamepieceInIntake) {
+			lastIntookSimulatedGampieceTime = Intake.getInstance().getStateTime();
+		}
+		return gamepieceInIntake;
+	}
 }