Manager

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

@@ -29,6 +29,7 @@ public enum RobotMode {
     }
     public static final RobotMode ROBOT_MODE = "Crash".equals(System.getenv("CI_NAME")) ? RobotMode.SIM : RobotMode.SIM;
     public static final double SIM_DELTA_TIME = 0.02;
+    public static final int MAX_PIECES = 6;
 
     public static class Controllers {
         public static final XboxController DRIVER_CONTROLLER = new XboxController(0);
@@ -37,6 +38,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;
     }
 
 

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

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

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

@@ -2,29 +2,34 @@
 
 import org.littletonrobotics.junction.Logger;
 
-import frc.robot.Constants;
+import frc.robot.GlobalConstants;
 import frc.robot.pioneersLib.subsystem.Subsystem;
 
 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 (Constants.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() {
@@ -35,6 +40,7 @@ public void stop() {
         io.stop();
     }
 
+
     @Override
     public void periodic() {
        super.periodic();
@@ -48,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

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

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

@@ -4,15 +4,14 @@
 
 import com.pathplanner.lib.config.PIDConstants;
 
-import edu.wpi.first.math.geometry.Pose3d;
 
 public interface IndexerIO {
     @AutoLog
 	public static class IndexerIOInputs {
         public double wheelSpeed;
         public double wheelSetpoint;
 
-        public boolean[] beamBreakArray = new boolean[6];
+        public boolean[] beamBreakArray = new boolean[IndexerConstants.NUM_BEAM_BREAK];
 	}
 
     public default void updateInputs(IndexerIOInputs inputs) {}

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

@@ -18,7 +18,7 @@ public class IndexerIOSim implements IndexerIO {
     private boolean[] beamBreakArray = {false, false, false, false, false, false};
 
     public IndexerIOSim() {
-        wheelMotorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.000001, 1), DCMotor.getKrakenX60Foc(IndexerConstants.NUM_SPINNER_MOTORS), 0);
+        wheelMotorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.000001, 1), DCMotor.getKrakenX60Foc(IndexerConstants.NUM_SPINNER_MOTORS));
 
         speedController = new PIDController(0, 0, 0);
     }

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

@@ -30,8 +30,8 @@ public IndexerIOTalonFX() {
 
         speedController = new PIDController(0, 0, 0);
 
-        beamBreaks = new DigitalInput[6];
-        for (int i = 0; i < 6; 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,
@@ -53,7 +53,7 @@ public void updateInputs(IndexerIOInputs inputs) {
         inputs.wheelSetpoint = wheelSpeedpoint;
         inputs.wheelSpeed = wheelMotor.getVelocity().getValueAsDouble();
 
-        for (int i = 0; i < 6; i++) {
+        for (int i = 0; i < beamBreakDebouncers.length; i++) {
             inputs.beamBreakArray[i] = !beamBreakDebouncers[i].calculate(beamBreaks[i].get());
         }
     }
@@ -76,7 +76,7 @@ public void configurePID(PIDConstants constants) {
 
     @Override
     public int getNumberOfPieces() {
-        for (int i = 0; i < 6; 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

@@ -10,7 +10,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;
 
@@ -23,6 +23,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;
 
@@ -39,15 +40,35 @@ private Manager() {
             addRunnableTrigger(() -> {commandScheduler.schedule(sysIdSubsystem.sysIdQuasistatic(Direction.kForward));}, () -> TEST_CONTROLLER.getXButtonPressed());
             addRunnableTrigger(() -> {commandScheduler.schedule(sysIdSubsystem.sysIdQuasistatic(Direction.kReverse));}, () -> TEST_CONTROLLER.getYButtonPressed());
         }
-        addTrigger(ManagerStates.IDLE, ManagerStates.SCORING_HIGH, () -> Controllers.DRIVER_CONTROLLER.getXButtonPressed());
-        addTrigger(ManagerStates.SCORING_HIGH, ManagerStates.IDLE, () -> 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.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, () -> Controllers.DRIVER_CONTROLLER.getAButtonPressed());
-        addTrigger(ManagerStates.INTAKING, ManagerStates.IDLE, () -> Controllers.DRIVER_CONTROLLER.getAButtonPressed());
+        //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());
+
+        //from outtaking
+        addTrigger(ManagerStates.OUTTAKING, ManagerStates.IDLE, () -> Controllers.DRIVER_CONTROLLER.getAButtonPressed());
+        addTrigger(ManagerStates.OUTTAKING, ManagerStates.INTAKING, () -> Controllers.DRIVER_CONTROLLER.getYButtonPressed());
+
+        //GOING HIGH
+        addTrigger(ManagerStates.GOING_HIGH, ManagerStates.SCORING_HIGH, () -> elevator.nearTarget());
+
+        //SCORING HIGH
+        addTrigger(ManagerStates.SCORING_HIGH, ManagerStates.IDLE, () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexer.isEmpty());
+
+        //GOING MID
+        addTrigger(ManagerStates.GOING_MID, ManagerStates.SCORING_MID, () -> elevator.nearTarget());
+
+        //SCORING MID
+        addTrigger(ManagerStates.SCORING_MID, ManagerStates.IDLE, () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexer.isEmpty());
     }
 
     public static Manager getInstance() {
-        if (instance == null) {
+        if (instance == null) {     
             instance = new Manager();
         }
         return instance;
@@ -58,13 +79,19 @@ public void runState() {
         Logger.recordOutput("Manager/State", getState().getStateString());
         Logger.recordOutput("Manager/State Time", getStateTime());
 
+        if(Controllers.OPERATOR_CONTROLLER.getXButtonPressed()) {
+            setState(ManagerStates.IDLE);
+        }
+
         elevator.setState(getState().getElevatorState());
         intake.setState(getState().getIntakeState());
+        indexer.setState(getState().getIndexerState());
 
         drive.periodic();
         vision.periodic();
         elevator.periodic();
         intake.periodic();
+        indexer.periodic();
 
         // Other subsystem periodics go here
     }

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

@@ -2,22 +2,29 @@
 
 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() {
@@ -29,4 +36,7 @@ protected ElevatorStates getElevatorState() {
     protected IntakeStates getIntakeState() {
         return intakeState;
     }
+    protected IndexerStates getIndexerState() {
+        return indexerState;
+    }
 }