Merge pull request #5 from FRC-7525/manager

Manager

src/main/java/frc/robot/GlobalConstants.java

@@ -31,6 +31,7 @@ public enum RobotMode {
 		? RobotMode.SIM
 		: RobotMode.SIM;
 	public static final double SIM_DELTA_TIME = 0.02;
+	public static final int MAX_PIECES = 6;
 
 	public static class Controllers {
 
@@ -40,6 +41,7 @@ public static class Controllers {
 
 		// NOTE: Set to 0.1 on trash controllers
 		public static final double DEADBAND = 0.01;
+		public static final double TRIGGERS_REGISTER_POINT = 0.5;
 	}
 
 	public static class Drive {

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

@@ -43,4 +43,8 @@ protected void runState() {
 		io.updateInputs(inputs);
 		Logger.processInputs(ElevatorConstants.SUBSYSTEM_NAME, inputs);
 	}
+
+	public boolean nearTarget() {
+		return io.nearTarget();
+	}
 }

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

@@ -6,26 +6,31 @@
 
 public class Indexer extends Subsystem<IndexerStates> {
 
+	private static Indexer instance;
 	private IndexerIO io;
 	private IndexerIOInputsAutoLogged inputs;
 
-	public Indexer(IndexerIO io) {
+	private Indexer() {
 		super("Indexer", IndexerStates.OFF);
-		this.io = io;
-
 		addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () ->
 			io.nextSensorTriggered()
 		);
 		addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> true); //TODO: actually get intake beam break
 
-		switch (GlobalConstants.ROBOT_MODE) {
-			case REAL:
-				break;
-			case SIM:
-				break;
-			default:
-				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();
+	}
+
+	public static Indexer getInstance() {
+		if (instance == null) {
+			instance = new Indexer();
 		}
+		return instance;
 	}
 
 	protected void runState() {
@@ -49,4 +54,8 @@ public void periodic() {
 	public int getNumberOfPieces() {
 		return io.getNumberOfPieces();
 	}
+
+	public boolean isEmpty() {
+		return io.getNumberOfPieces() == 0;
+	}
 }

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

@@ -4,6 +4,7 @@ public final class IndexerConstants {
 
 	public static final int SPINNER_ID = 1;
 
+	public static final int NUM_BEAM_BREAK = 6;
 	public static final int[] BEAM_BREAK_PORTS = { 1, 2, 3, 4, 5, 6 };
 	public static final double DEBOUNCE_TIME = 0.25;
 

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

@@ -1,19 +1,15 @@
 package frc.robot.subsystems.Indexer;
 
-import static frc.robot.subsystems.Indexer.IndexerConstants.*;
-
 import com.pathplanner.lib.config.PIDConstants;
-import edu.wpi.first.math.geometry.Pose3d;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface IndexerIO {
 	@AutoLog
 	public static class IndexerIOInputs {
 
+		public boolean[] beamBreakArray = new boolean[IndexerConstants.NUM_BEAM_BREAK];
 		public double wheelSpeed;
 		public double wheelSetpoint;
-
-		public boolean[] beamBreakArray = new boolean[MAX_GAME_PIECES];
 	}
 
 	public default void updateInputs(IndexerIOInputs inputs) {}

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

@@ -20,8 +20,7 @@ public class IndexerIOSim implements IndexerIO {
 	public IndexerIOSim() {
 		wheelMotorSim = new DCMotorSim(
 			LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.000001, 1),
-			DCMotor.getKrakenX60Foc(IndexerConstants.NUM_SPINNER_MOTORS),
-			0
+			DCMotor.getKrakenX60(IndexerConstants.NUM_SPINNER_MOTORS)
 		);
 
 		speedController = new PIDController(0, 0, 0);

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

@@ -1,7 +1,5 @@
 package frc.robot.subsystems.Indexer;
 
-import static frc.robot.subsystems.Indexer.IndexerConstants.*;
-
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.pathplanner.lib.config.PIDConstants;
 import edu.wpi.first.math.controller.PIDController;
@@ -32,8 +30,8 @@ public IndexerIOTalonFX() {
 
 		speedController = new PIDController(0, 0, 0);
 
-		beamBreaks = new DigitalInput[MAX_GAME_PIECES];
-		for (int i = 0; i < MAX_GAME_PIECES; i++) {
+		beamBreaks = new DigitalInput[IndexerConstants.NUM_BEAM_BREAK];
+		for (int i = 0; i < beamBreaks.length; i++) {
 			beamBreaks[i] = new DigitalInput(IndexerConstants.BEAM_BREAK_PORTS[i]);
 			beamBreakDebouncers[i] = new Debouncer(
 				IndexerConstants.DEBOUNCE_TIME,
@@ -58,7 +56,7 @@ public void updateInputs(IndexerIOInputs inputs) {
 		inputs.wheelSetpoint = wheelSpeedpoint;
 		inputs.wheelSpeed = wheelMotor.getVelocity().getValueAsDouble();
 
-		for (int i = 0; i < MAX_GAME_PIECES; i++) {
+		for (int i = 0; i < beamBreakDebouncers.length; i++) {
 			inputs.beamBreakArray[i] = !beamBreakDebouncers[i].calculate(beamBreaks[i].get());
 		}
 	}
@@ -81,7 +79,7 @@ public void configurePID(PIDConstants constants) {
 
 	@Override
 	public int getNumberOfPieces() {
-		for (int i = 0; i < MAX_GAME_PIECES; i++) {
+		for (int i = 0; i < beamBreakDebouncers.length; i++) {
 			if (beamBreakDebouncers[i].calculate(beamBreaks[i].get())) {
 				currentNumOfPieces = i;
 				return i;

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

@@ -8,7 +8,7 @@
 import frc.robot.pioneersLib.subsystem.Subsystem;
 import frc.robot.subsystems.Drive.Drive;
 import frc.robot.subsystems.Elevator.Elevator;
-import frc.robot.subsystems.Elevator.ElevatorStates;
+import frc.robot.subsystems.Indexer.Indexer;
 import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.Vision.Vision;
 import org.littletonrobotics.junction.Logger;
@@ -22,6 +22,7 @@ public class Manager extends Subsystem<ManagerStates> {
 	private final Elevator elevator = Elevator.getInstance();
 	private final CommandScheduler commandScheduler = CommandScheduler.getInstance();
 	private final Intake intake = Intake.getInstance();
+	private final Indexer indexer = Indexer.getInstance();
 	// Change to change the subsystem that gets tested (has runnable sysID tests) saftey ish
 	private final Subsystem<?> sysIdSubsystem = drive;
 
@@ -56,19 +57,75 @@ private Manager() {
 				() -> TEST_CONTROLLER.getYButtonPressed()
 			);
 		}
-		addTrigger(ManagerStates.IDLE, ManagerStates.SCORING_HIGH, () ->
-			Controllers.DRIVER_CONTROLLER.getXButtonPressed()
+		// from idle
+		addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () ->
+			Controllers.DRIVER_CONTROLLER.getBButtonPressed()
+		);
+		addTrigger(ManagerStates.IDLE, ManagerStates.OUTTAKING, () ->
+			Controllers.DRIVER_CONTROLLER.getAButtonPressed()
 		);
-		addTrigger(ManagerStates.SCORING_HIGH, ManagerStates.IDLE, () ->
-			Controllers.DRIVER_CONTROLLER.getXButtonPressed()
+		addTrigger(
+			ManagerStates.IDLE,
+			ManagerStates.GOING_HIGH,
+			() ->
+				Controllers.DRIVER_CONTROLLER.getRightTriggerAxis() >
+				Controllers.TRIGGERS_REGISTER_POINT
+		);
+		addTrigger(
+			ManagerStates.IDLE,
+			ManagerStates.GOING_MID,
+			() ->
+				Controllers.DRIVER_CONTROLLER.getLeftTriggerAxis() >
+				Controllers.TRIGGERS_REGISTER_POINT
 		);
 
-		addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () ->
+		// from intaking
+		addTrigger(
+			ManagerStates.INTAKING,
+			ManagerStates.IDLE,
+			() ->
+				Controllers.DRIVER_CONTROLLER.getBButtonPressed() ||
+				Controllers.OPERATOR_CONTROLLER.getAButtonPressed() ||
+				indexer.getNumberOfPieces() == MAX_PIECES
+		);
+		addTrigger(ManagerStates.INTAKING, ManagerStates.OUTTAKING, () ->
 			Controllers.DRIVER_CONTROLLER.getAButtonPressed()
 		);
-		addTrigger(ManagerStates.INTAKING, ManagerStates.IDLE, () ->
+
+		// from outtaking
+		addTrigger(ManagerStates.OUTTAKING, ManagerStates.IDLE, () ->
 			Controllers.DRIVER_CONTROLLER.getAButtonPressed()
 		);
+		addTrigger(ManagerStates.OUTTAKING, ManagerStates.INTAKING, () ->
+			Controllers.DRIVER_CONTROLLER.getYButtonPressed()
+		);
+
+		// form going high
+		addTrigger(ManagerStates.GOING_HIGH, ManagerStates.SCORING_HIGH, () -> elevator.nearTarget()
+		);
+
+		// from scoring high
+		addTrigger(
+			ManagerStates.SCORING_HIGH,
+			ManagerStates.IDLE,
+			() -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexer.isEmpty()
+		);
+
+		// from going mid
+		addTrigger(ManagerStates.GOING_MID, ManagerStates.SCORING_MID, () -> elevator.nearTarget());
+
+		// from scoring mid
+		addTrigger(
+			ManagerStates.SCORING_MID,
+			ManagerStates.IDLE,
+			() -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexer.isEmpty()
+		);
+
+		// return to idle
+		addRunnableTrigger(
+			() -> setState(ManagerStates.IDLE),
+			() -> Controllers.OPERATOR_CONTROLLER.getXButtonPressed()
+		);
 	}
 
 	public static Manager getInstance() {
@@ -80,16 +137,20 @@ public static Manager getInstance() {
 
 	@Override
 	public void runState() {
-		Logger.recordOutput("Manager/State", getState().getStateString());
-		Logger.recordOutput("Manager/State Time", getStateTime());
+		Logger.recordOutput(
+			ManagerConstants.SUBSYSTEM_NAME + "/State",
+			getState().getStateString()
+		);
+		Logger.recordOutput(ManagerConstants.SUBSYSTEM_NAME + "/State Time", getStateTime());
 
 		elevator.setState(getState().getElevatorState());
 		intake.setState(getState().getIntakeState());
+		indexer.setState(getState().getIndexerState());
 
 		drive.periodic();
 		vision.periodic();
 		elevator.periodic();
 		intake.periodic();
-		// Other subsystem periodics go here
+		indexer.periodic();
 	}
 }

src/main/java/frc/robot/subsystems/Manager/ManagerConstants.java

@@ -0,0 +1,6 @@
+package frc.robot.subsystems.Manager;
+
+public final class ManagerConstants {
+
+	public static final String SUBSYSTEM_NAME = "Manager";
+}

src/main/java/frc/robot/subsystems/Manager/ManagerStates.java

@@ -2,22 +2,39 @@
 
 import frc.robot.pioneersLib.subsystem.SubsystemStates;
 import frc.robot.subsystems.Elevator.ElevatorStates;
+import frc.robot.subsystems.Indexer.IndexerStates;
 import frc.robot.subsystems.Intake.IntakeStates;
 
 public enum ManagerStates implements SubsystemStates {
-	IDLE("Idle", ElevatorStates.IDLE, IntakeStates.IDLE),
-	INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING),
-	SCORING_HIGH("SCORING HIGH", ElevatorStates.HIGH, IntakeStates.IDLE);
+	IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerStates.OFF),
+	INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerStates.AUTONOMOUS_ON),
+	OUTTAKING(
+		"OUTTAKING",
+		ElevatorStates.IDLE,
+		IntakeStates.OUTTAKING,
+		IndexerStates.AUTONOMOUS_ON
+	),
+	GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerStates.OFF),
+	SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerStates.SCORING),
+	GOING_HIGH("GOING HIGH", ElevatorStates.HIGH, IntakeStates.INTAKING, IndexerStates.OFF),
+	SCORING_HIGH("SCORING HIGH", ElevatorStates.HIGH, IntakeStates.IDLE, IndexerStates.SCORING);
 
-	ManagerStates(String stateString, ElevatorStates elevatorState, IntakeStates intakeState) {
+	ManagerStates(
+		String stateString,
+		ElevatorStates elevatorState,
+		IntakeStates intakeState,
+		IndexerStates indexerState
+	) {
 		this.stateString = stateString;
 		this.elevatorState = elevatorState;
 		this.intakeState = intakeState;
+		this.indexerState = indexerState;
 	}
 
 	private String stateString;
-	private ElevatorStates elevatorState;
 	private IntakeStates intakeState;
+	private ElevatorStates elevatorState;
+	private IndexerStates indexerState;
 
 	@Override
 	public String getStateString() {
@@ -31,4 +48,8 @@ protected ElevatorStates getElevatorState() {
 	protected IntakeStates getIntakeState() {
 		return intakeState;
 	}
+
+	protected IndexerStates getIndexerState() {
+		return indexerState;
+	}
 }