From 0a9acaa5c86ac1b4e9da7bab4ae2cf20a1741d93 Mon Sep 17 00:00:00 2001 From: Otto Date: Wed, 1 Jan 2025 21:16:09 -0600 Subject: [PATCH 01/14] Enhance Indexer and Intake subsystems with new state management and simulation features zzzzzzzzzzzz --- .../pioneersLib/subsystem/Subsystem.java | 10 +++++++ .../frc/robot/subsystems/Indexer/Indexer.java | 26 ++++++++++++++++--- .../subsystems/Indexer/IndexerConstants.java | 6 +++++ .../subsystems/Indexer/IndexerIOSim.java | 4 --- .../frc/robot/subsystems/Intake/Intake.java | 18 +++++++++++-- .../subsystems/Intake/IntakeConstants.java | 6 ++++- .../frc/robot/subsystems/Intake/IntakeIO.java | 10 ++++--- .../robot/subsystems/Intake/IntakeIOReal.java | 8 ++++++ .../frc/robot/subsystems/Manager/Manager.java | 1 + .../subsystems/Manager/ManagerStates.java | 2 +- 10 files changed, 77 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/pioneersLib/subsystem/Subsystem.java b/src/main/java/frc/robot/pioneersLib/subsystem/Subsystem.java index f21fe4c..23d49d8 100644 --- a/src/main/java/frc/robot/pioneersLib/subsystem/Subsystem.java +++ b/src/main/java/frc/robot/pioneersLib/subsystem/Subsystem.java @@ -101,6 +101,16 @@ public void setState(StateType state) { this.state = state; } + // TODO: This is mad goofy + // For if you need to override set state + public void manualSetState(StateType state) { + this.state = state; + } + + public void resetStateTimer() { + stateTimer.reset(); + } + /** * Gets amount of time the state machine has been in the current state. * diff --git a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java index b357704..5d5e676 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java @@ -1,7 +1,12 @@ package frc.robot.subsystems.Indexer; import frc.robot.GlobalConstants; +import frc.robot.GlobalConstants.RobotMode; import frc.robot.pioneersLib.subsystem.Subsystem; +import frc.robot.subsystems.Intake.Intake; + +import static edu.wpi.first.units.Units.Seconds; + import org.littletonrobotics.junction.Logger; public class Indexer extends Subsystem { @@ -12,10 +17,15 @@ public class Indexer extends Subsystem { private Indexer() { super("Indexer", IndexerStates.OFF); - addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () -> - io.nextSensorTriggered() + + addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> + {if (GlobalConstants.ROBOT_MODE == RobotMode.SIM) { + return getStateTime() > IndexerConstants.SIMULATED_INDEXING_TIME.in(Seconds); + } else { + return io.nextSensorTriggered(); + }} ); - addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> true); //TODO: actually get intake beam break + addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () -> Intake.getInstance().hasGamepiece()); this.io = switch (GlobalConstants.ROBOT_MODE) { case SIM -> new IndexerIOSim(); @@ -58,4 +68,14 @@ public int getNumberOfPieces() { public boolean isEmpty() { return io.getNumberOfPieces() == 0; } + + // TODO: THIS IS ACTUALLY SO GOOFY + @Override + public void setState(IndexerStates state) { + if (getState() != state) resetStateTimer(); + if (state == IndexerStates.AUTONOMOUS_OFF && getState() == IndexerStates.AUTONOMOUS_ON) { + return; + } + manualSetState(state); + } } diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerConstants.java index 7f4ef90..c1c235f 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerConstants.java +++ b/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; } diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java index c4b9098..a70d9ce 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java @@ -61,8 +61,4 @@ public double getSpeed() { public void configurePID(PIDConstants constants) { speedController = new PIDController(constants.kP, constants.kI, constants.kD); } - - public boolean[] getBeamBreakArray() { - return beamBreakArray; - } } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 25a3910..f356506 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -1,14 +1,16 @@ package frc.robot.subsystems.Intake; +import static edu.wpi.first.units.Units.Seconds; import static frc.robot.GlobalConstants.ROBOT_MODE; +import frc.robot.GlobalConstants; import frc.robot.pioneersLib.subsystem.Subsystem; import org.littletonrobotics.junction.Logger; public class Intake extends Subsystem { - IntakeIO io; - IntakeIOInputsAutoLogged inputs; + private IntakeIO io; + private IntakeIOInputsAutoLogged inputs; private static Intake instance; private Intake(IntakeIO io) { @@ -39,4 +41,16 @@ public static Intake getInstance() { } return instance; } + + public boolean hasGamepiece() { + if (getState() == IntakeStates.INTAKING) { + if (GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.REAL || GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.TESTING) { + return io.hasGamepiece(); + } else { + // TODO, happen more than once pls + return getStateTime() > IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); + } + } + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java index ec6f1c3..4627ea3 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java +++ b/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,14 @@ 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); } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java index 88a3e69..0ece6bf 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java @@ -2,6 +2,8 @@ import org.littletonrobotics.junction.AutoLog; +import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Default; + public interface IntakeIO { @AutoLog public static class IntakeIOInputs { @@ -15,9 +17,11 @@ public static class IntakeIOInputs { public double wheelSpeedSetpoint; } - public void updateInputs(IntakeIOInputs input); + public default void updateInputs(IntakeIOInputs input) {}; + + public default void setPivotSetpoint(double pivotSetpoint) {}; - public void setPivotSetpoint(double pivotSetpoint); + public default void setWheelSpeed(double wheelSpeed) {}; - public void setWheelSpeed(double wheelSpeed); + public default boolean hasGamepiece() {return false;}; } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOReal.java index bdde609..f727b64 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIOReal.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/subsystems/Manager/Manager.java b/src/main/java/frc/robot/subsystems/Manager/Manager.java index 88e58c8..31cd6fd 100644 --- a/src/main/java/frc/robot/subsystems/Manager/Manager.java +++ b/src/main/java/frc/robot/subsystems/Manager/Manager.java @@ -57,6 +57,7 @@ private Manager() { () -> TEST_CONTROLLER.getYButtonPressed() ); } + // from idle addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () -> Controllers.DRIVER_CONTROLLER.getBButtonPressed() diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index 225b828..744e326 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -7,7 +7,7 @@ public enum ManagerStates implements SubsystemStates { IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerStates.OFF), - INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerStates.AUTONOMOUS_ON), + INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerStates.AUTONOMOUS_OFF), OUTTAKING( "OUTTAKING", ElevatorStates.IDLE, From 377674bcd28ca2fd87101e6c8c3386a057070adf Mon Sep 17 00:00:00 2001 From: github-actions Date: Thu, 2 Jan 2025 03:17:09 +0000 Subject: [PATCH 02/14] Apply Prettier format --- .../frc/robot/subsystems/Indexer/Indexer.java | 16 ++++++------ .../frc/robot/subsystems/Intake/Intake.java | 25 +++++++++++-------- .../subsystems/Intake/IntakeConstants.java | 3 ++- .../frc/robot/subsystems/Intake/IntakeIO.java | 13 +++++----- .../frc/robot/subsystems/Manager/Manager.java | 2 +- 5 files changed, 32 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java index 5d5e676..a65a1d2 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java @@ -1,12 +1,11 @@ package frc.robot.subsystems.Indexer; +import static edu.wpi.first.units.Units.Seconds; + import frc.robot.GlobalConstants; import frc.robot.GlobalConstants.RobotMode; import frc.robot.pioneersLib.subsystem.Subsystem; import frc.robot.subsystems.Intake.Intake; - -import static edu.wpi.first.units.Units.Seconds; - import org.littletonrobotics.junction.Logger; public class Indexer extends Subsystem { @@ -17,15 +16,16 @@ public class Indexer extends Subsystem { private Indexer() { super("Indexer", IndexerStates.OFF); - - addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> - {if (GlobalConstants.ROBOT_MODE == RobotMode.SIM) { + addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> { + if (GlobalConstants.ROBOT_MODE == RobotMode.SIM) { return getStateTime() > IndexerConstants.SIMULATED_INDEXING_TIME.in(Seconds); } else { return io.nextSensorTriggered(); - }} + } + }); + addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () -> + Intake.getInstance().hasGamepiece() ); - addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () -> Intake.getInstance().hasGamepiece()); this.io = switch (GlobalConstants.ROBOT_MODE) { case SIM -> new IndexerIOSim(); diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index f356506..ca8a506 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -42,15 +42,18 @@ public static Intake getInstance() { return instance; } - public boolean hasGamepiece() { - if (getState() == IntakeStates.INTAKING) { - if (GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.REAL || GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.TESTING) { - return io.hasGamepiece(); - } else { - // TODO, happen more than once pls - return getStateTime() > IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); - } - } - return false; - } + public boolean hasGamepiece() { + if (getState() == IntakeStates.INTAKING) { + if ( + GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.REAL || + GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.TESTING + ) { + return io.hasGamepiece(); + } else { + // TODO, happen more than once pls + return getStateTime() > IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); + } + } + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java index 4627ea3..7241c26 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java @@ -40,11 +40,12 @@ 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; diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java index 0ece6bf..fbb09ff 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java @@ -1,8 +1,7 @@ package frc.robot.subsystems.Intake; -import org.littletonrobotics.junction.AutoLog; - import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Default; +import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { @AutoLog @@ -17,11 +16,13 @@ public static class IntakeIOInputs { public double wheelSpeedSetpoint; } - public default void updateInputs(IntakeIOInputs input) {}; + public default void updateInputs(IntakeIOInputs input) {} - public default void setPivotSetpoint(double pivotSetpoint) {}; + public default void setPivotSetpoint(double pivotSetpoint) {} - public default void setWheelSpeed(double wheelSpeed) {}; + public default void setWheelSpeed(double wheelSpeed) {} - public default boolean hasGamepiece() {return false;}; + public default boolean hasGamepiece() { + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/Manager/Manager.java b/src/main/java/frc/robot/subsystems/Manager/Manager.java index 31cd6fd..a29f476 100644 --- a/src/main/java/frc/robot/subsystems/Manager/Manager.java +++ b/src/main/java/frc/robot/subsystems/Manager/Manager.java @@ -57,7 +57,7 @@ private Manager() { () -> TEST_CONTROLLER.getYButtonPressed() ); } - + // from idle addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () -> Controllers.DRIVER_CONTROLLER.getBButtonPressed() From a4ca5297d4290acb1c3c10aa3894998ff19be53c Mon Sep 17 00:00:00 2001 From: Otto Date: Wed, 1 Jan 2025 21:20:53 -0600 Subject: [PATCH 03/14] Fix a TODO --- .../java/frc/robot/subsystems/Intake/Intake.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index f356506..bec72f3 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -13,14 +13,21 @@ public class Intake extends Subsystem { private IntakeIOInputsAutoLogged inputs; private static Intake instance; + // Strictly for sim + private double lastIntookSimulatedGampieceTime; + private Intake(IntakeIO io) { super("Intake", IntakeStates.IDLE); this.io = io; inputs = new IntakeIOInputsAutoLogged(); + lastIntookSimulatedGampieceTime = 0.0; } @Override protected void runState() { + // ehhhh, yeah idk if this will work, ig its in the same loop as the reset so it should theoretically work + if (getStateTime() == 0) {lastIntookSimulatedGampieceTime = 0.0;} + io.setPivotSetpoint(getState().getPivotSetpoint()); io.setWheelSpeed(getState().getWheelSpeedSetpoint()); @@ -47,8 +54,11 @@ public boolean hasGamepiece() { if (GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.REAL || GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.TESTING) { return io.hasGamepiece(); } else { - // TODO, happen more than once pls - return getStateTime() > IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); + boolean gamepieceInIntake = getStateTime() - lastIntookSimulatedGampieceTime > IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); + if (gamepieceInIntake) { + lastIntookSimulatedGampieceTime = getStateTime(); + } + return gamepieceInIntake; } } return false; From eed1354176d952061c5a03bf554f60a3283b4a14 Mon Sep 17 00:00:00 2001 From: github-actions Date: Thu, 2 Jan 2025 03:21:22 +0000 Subject: [PATCH 04/14] Apply Prettier format --- .../frc/robot/subsystems/Intake/Intake.java | 45 +++++++++++-------- 1 file changed, 26 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index bec72f3..d8f38e6 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -13,20 +13,22 @@ public class Intake extends Subsystem { private IntakeIOInputsAutoLogged inputs; private static Intake instance; - // Strictly for sim - private double lastIntookSimulatedGampieceTime; + // Strictly for sim + private double lastIntookSimulatedGampieceTime; private Intake(IntakeIO io) { super("Intake", IntakeStates.IDLE); this.io = io; inputs = new IntakeIOInputsAutoLogged(); - lastIntookSimulatedGampieceTime = 0.0; + lastIntookSimulatedGampieceTime = 0.0; } @Override protected void runState() { - // ehhhh, yeah idk if this will work, ig its in the same loop as the reset so it should theoretically work - if (getStateTime() == 0) {lastIntookSimulatedGampieceTime = 0.0;} + // ehhhh, yeah idk if this will work, ig its in the same loop as the reset so it should theoretically work + if (getStateTime() == 0) { + lastIntookSimulatedGampieceTime = 0.0; + } io.setPivotSetpoint(getState().getPivotSetpoint()); io.setWheelSpeed(getState().getWheelSpeedSetpoint()); @@ -49,18 +51,23 @@ public static Intake getInstance() { return instance; } - public boolean hasGamepiece() { - if (getState() == IntakeStates.INTAKING) { - if (GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.REAL || GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.TESTING) { - return io.hasGamepiece(); - } else { - boolean gamepieceInIntake = getStateTime() - lastIntookSimulatedGampieceTime > IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); - if (gamepieceInIntake) { - lastIntookSimulatedGampieceTime = getStateTime(); - } - return gamepieceInIntake; - } - } - return false; - } + public boolean hasGamepiece() { + if (getState() == IntakeStates.INTAKING) { + if ( + GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.REAL || + GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.TESTING + ) { + return io.hasGamepiece(); + } else { + boolean gamepieceInIntake = + getStateTime() - lastIntookSimulatedGampieceTime > + IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); + if (gamepieceInIntake) { + lastIntookSimulatedGampieceTime = getStateTime(); + } + return gamepieceInIntake; + } + } + return false; + } } From 5e308c8a0ee22bd7b06d4b5d33a6432d6116a199 Mon Sep 17 00:00:00 2001 From: Otto Date: Wed, 1 Jan 2025 23:10:13 -0600 Subject: [PATCH 05/14] Had some medium thoughts per minute here --- .../frc/robot/subsystems/Indexer/Indexer.java | 25 +-------- .../subsystems/Indexer/IndexerIOSim.java | 7 +++ .../subsystems/Indexer/IndexerIOTalonFX.java | 2 +- .../IndexerManager/IndexerManager.java | 52 +++++++++++++++++++ .../IndexerManager/IndexerManagerStates.java | 27 ++++++++++ .../subsystems/Indexer/IndexerStates.java | 3 +- .../frc/robot/subsystems/Intake/Intake.java | 17 ++---- .../robot/subsystems/Intake/IntakeIOSim.java | 12 +++++ .../frc/robot/subsystems/Manager/Manager.java | 14 ++--- .../subsystems/Manager/ManagerStates.java | 25 ++++----- 10 files changed, 127 insertions(+), 57 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java create mode 100644 src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java diff --git a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java index a65a1d2..01ea3f3 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java @@ -1,9 +1,6 @@ package frc.robot.subsystems.Indexer; -import static edu.wpi.first.units.Units.Seconds; - import frc.robot.GlobalConstants; -import frc.robot.GlobalConstants.RobotMode; import frc.robot.pioneersLib.subsystem.Subsystem; import frc.robot.subsystems.Intake.Intake; import org.littletonrobotics.junction.Logger; @@ -16,16 +13,6 @@ public class Indexer extends Subsystem { private Indexer() { super("Indexer", IndexerStates.OFF); - addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> { - if (GlobalConstants.ROBOT_MODE == RobotMode.SIM) { - return getStateTime() > IndexerConstants.SIMULATED_INDEXING_TIME.in(Seconds); - } else { - return io.nextSensorTriggered(); - } - }); - addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () -> - Intake.getInstance().hasGamepiece() - ); this.io = switch (GlobalConstants.ROBOT_MODE) { case SIM -> new IndexerIOSim(); @@ -55,8 +42,6 @@ public void stop() { public void periodic() { super.periodic(); - //TODO: Add output managing - Logger.processInputs("Indexer", inputs); io.updateInputs(inputs); } @@ -69,13 +54,7 @@ public boolean isEmpty() { return io.getNumberOfPieces() == 0; } - // TODO: THIS IS ACTUALLY SO GOOFY - @Override - public void setState(IndexerStates state) { - if (getState() != state) resetStateTimer(); - if (state == IndexerStates.AUTONOMOUS_OFF && getState() == IndexerStates.AUTONOMOUS_ON) { - return; - } - manualSetState(state); + public double getStateTime() { + return getStateTime(); } } diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java index a70d9ce..4d415c9 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java +++ b/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; @@ -61,4 +63,9 @@ public double getSpeed() { public void configurePID(PIDConstants constants) { speedController = new PIDController(constants.kP, constants.kI, constants.kD); } + + @Override + public boolean nextSensorTriggered() { + return Indexer.getInstance().getStateTime() > IndexerConstants.SIMULATED_INDEXING_TIME.in(Seconds); + } } diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerIOTalonFX.java index 28d00d0..5e17b33 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerIOTalonFX.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java new file mode 100644 index 0000000..0f8003a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java @@ -0,0 +1,52 @@ +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 IndexerManager extends Subsystem { + + private static IndexerManager instance; + private final Indexer indexer = Indexer.getInstance(); + + // Those who know + private IndexerManager() { + super("Indexer Manager", IndexerManagerStates.OFF); + } + + public static IndexerManager getInstance() { + if (instance == null) { + instance = new IndexerManager(); + } + 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() { + if (getState() == IndexerManagerStates.AUTONOMOUS) { + indexer.setState(IndexerManagerStates.AUTONOMOUS.getIndexerState()); + if (Indexer.getInstance().) + + } else { + indexer.setState(getState().getIndexerState()); + } + } + + // addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> io.nextSensorTriggered()); + // addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () -> + // Intake.getInstance().hasGamepiece() + // ); +} diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java new file mode 100644 index 0000000..49e1883 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.Indexer.IndexerManager; + +import frc.robot.pioneersLib.subsystem.SubsystemStates; +import frc.robot.subsystems.Indexer.IndexerStates; + +public enum IndexerManagerStates implements SubsystemStates { + SCORING("SCORING", IndexerStates.SCORING), + OFF("OFF", IndexerStates.OFF), + AUTONOMOUS("Autonomous", IndexerStates.OFF); + + IndexerManagerStates(String stateString, IndexerStates indexerState) { + this.stateString = stateString; + this.indexerState = indexerState; + } + + private String stateString; + private IndexerStates indexerState; + + @Override + public String getStateString() { + return stateString; + } + + protected IndexerStates getIndexerState() { + return indexerState; + } +} diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java index 806439b..8089b46 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java @@ -4,8 +4,7 @@ public enum IndexerStates implements SubsystemStates { OFF("Off", IndexerConstants.OFF), - AUTONOMOUS_ON("Autonomous On", IndexerConstants.ON), - AUTONOMOUS_OFF("Autonomous Off", IndexerConstants.OFF), + INDEXING("Indexing", IndexerConstants.ON), SCORING("Scoring", IndexerConstants.ON); private String stateString; diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index bec72f3..836cdee 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -50,17 +50,10 @@ public static Intake getInstance() { } public boolean hasGamepiece() { - if (getState() == IntakeStates.INTAKING) { - if (GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.REAL || GlobalConstants.ROBOT_MODE == GlobalConstants.RobotMode.TESTING) { - return io.hasGamepiece(); - } else { - boolean gamepieceInIntake = getStateTime() - lastIntookSimulatedGampieceTime > IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); - if (gamepieceInIntake) { - lastIntookSimulatedGampieceTime = getStateTime(); - } - return gamepieceInIntake; - } - } - return false; + return io.hasGamepiece(); } + + public double getStateTime() { + return getStateTime(); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java index 5b751a7..3f08119 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java +++ b/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,12 @@ 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; + } } diff --git a/src/main/java/frc/robot/subsystems/Manager/Manager.java b/src/main/java/frc/robot/subsystems/Manager/Manager.java index a29f476..0d2bb21 100644 --- a/src/main/java/frc/robot/subsystems/Manager/Manager.java +++ b/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.Indexer.Indexer; +import frc.robot.subsystems.Indexer.IndexerManager.IndexerManager; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.Vision.Vision; import org.littletonrobotics.junction.Logger; @@ -22,7 +22,7 @@ public class Manager extends Subsystem { private final Elevator elevator = Elevator.getInstance(); private final CommandScheduler commandScheduler = CommandScheduler.getInstance(); private final Intake intake = Intake.getInstance(); - private final Indexer indexer = Indexer.getInstance(); + private final IndexerManager indexerManager = IndexerManager.getInstance(); // Change to change the subsystem that gets tested (has runnable sysID tests) saftey ish private final Subsystem sysIdSubsystem = drive; @@ -87,7 +87,7 @@ private Manager() { () -> Controllers.DRIVER_CONTROLLER.getBButtonPressed() || Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || - indexer.getNumberOfPieces() == MAX_PIECES + indexerManager.isFull() ); addTrigger(ManagerStates.INTAKING, ManagerStates.OUTTAKING, () -> Controllers.DRIVER_CONTROLLER.getAButtonPressed() @@ -109,7 +109,7 @@ private Manager() { addTrigger( ManagerStates.SCORING_HIGH, ManagerStates.IDLE, - () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexer.isEmpty() + () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexerManager.isEmpty() ); // from going mid @@ -119,7 +119,7 @@ private Manager() { addTrigger( ManagerStates.SCORING_MID, ManagerStates.IDLE, - () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexer.isEmpty() + () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexerManager.isEmpty() ); // return to idle @@ -146,12 +146,12 @@ public void runState() { elevator.setState(getState().getElevatorState()); intake.setState(getState().getIntakeState()); - indexer.setState(getState().getIndexerState()); + indexerManager.setState(getState().getIndexerManagerState()); drive.periodic(); vision.periodic(); elevator.periodic(); intake.periodic(); - indexer.periodic(); + indexerManager.periodic(); } } diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index 744e326..040e78d 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -3,38 +3,39 @@ import frc.robot.pioneersLib.subsystem.SubsystemStates; import frc.robot.subsystems.Elevator.ElevatorStates; import frc.robot.subsystems.Indexer.IndexerStates; +import frc.robot.subsystems.Indexer.IndexerManager.IndexerManagerStates; import frc.robot.subsystems.Intake.IntakeStates; public enum ManagerStates implements SubsystemStates { - IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerStates.OFF), - INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerStates.AUTONOMOUS_OFF), + IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerManagerStates.OFF), + INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerManagerStates.OFF), OUTTAKING( "OUTTAKING", ElevatorStates.IDLE, IntakeStates.OUTTAKING, - IndexerStates.AUTONOMOUS_ON + IndexerManagerStates.OFF ), - 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); + GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerManagerStates.OFF), + SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerManagerStates.SCORING), + GOING_HIGH("GOING HIGH", ElevatorStates.HIGH, IntakeStates.INTAKING, IndexerManagerStates.OFF), + SCORING_HIGH("SCORING HIGH", ElevatorStates.HIGH, IntakeStates.IDLE, IndexerManagerStates.SCORING); ManagerStates( String stateString, ElevatorStates elevatorState, IntakeStates intakeState, - IndexerStates indexerState + IndexerManagerStates indexerManagerState ) { this.stateString = stateString; this.elevatorState = elevatorState; this.intakeState = intakeState; - this.indexerState = indexerState; + this.indexerManagerState = indexerManagerState; } private String stateString; private IntakeStates intakeState; private ElevatorStates elevatorState; - private IndexerStates indexerState; + private IndexerManagerStates indexerManagerState; @Override public String getStateString() { @@ -49,7 +50,7 @@ protected IntakeStates getIntakeState() { return intakeState; } - protected IndexerStates getIndexerState() { - return indexerState; + protected IndexerManagerStates getIndexerManagerState() { + return indexerManagerState; } } From 2e67a738cbc5d4885a4d80981c90a6f090a25298 Mon Sep 17 00:00:00 2001 From: Otto Date: Fri, 3 Jan 2025 12:54:14 -0600 Subject: [PATCH 06/14] Lalalalala --- .../frc/robot/subsystems/Indexer/Indexer.java | 4 ++-- .../Indexer/IndexerManager/IndexerManager.java | 15 +++++++-------- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java index 01ea3f3..d6cd32f 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java @@ -50,8 +50,8 @@ public int getNumberOfPieces() { return io.getNumberOfPieces(); } - public boolean isEmpty() { - return io.getNumberOfPieces() == 0; + public boolean nextSensorTriggered() { + return io.nextSensorTriggered(); } public double getStateTime() { diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java index 0f8003a..03eab51 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java @@ -3,6 +3,8 @@ import frc.robot.pioneersLib.subsystem.Subsystem; import frc.robot.subsystems.Indexer.Indexer; import frc.robot.subsystems.Indexer.IndexerConstants; +import frc.robot.subsystems.Indexer.IndexerStates; +import frc.robot.subsystems.Intake.Intake; public class IndexerManager extends Subsystem { @@ -37,16 +39,13 @@ public boolean isFull() { @Override public void runState() { if (getState() == IndexerManagerStates.AUTONOMOUS) { - indexer.setState(IndexerManagerStates.AUTONOMOUS.getIndexerState()); - if (Indexer.getInstance().) - + if (indexer.getState() == IndexerStates.OFF && Intake.getInstance().hasGamepiece()) { + indexer.setState(IndexerStates.INDEXING); + } else if (indexer.getState() == IndexerStates.INDEXING && indexer.nextSensorTriggered()) { + indexer.setState(IndexerStates.OFF); + } } else { indexer.setState(getState().getIndexerState()); } } - - // addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> io.nextSensorTriggered()); - // addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () -> - // Intake.getInstance().hasGamepiece() - // ); } From b6030afa40ce6bf87521930627a00e1ff3159cbc Mon Sep 17 00:00:00 2001 From: github-actions Date: Fri, 3 Jan 2025 18:54:23 +0000 Subject: [PATCH 07/14] Apply Prettier format --- .../frc/robot/subsystems/Indexer/Indexer.java | 1 - .../subsystems/Indexer/IndexerIOSim.java | 5 +- .../IndexerManager/IndexerManager.java | 81 ++++++++++--------- .../IndexerManager/IndexerManagerStates.java | 32 ++++---- .../frc/robot/subsystems/Intake/Intake.java | 4 +- .../robot/subsystems/Intake/IntakeIOSim.java | 4 +- .../subsystems/Manager/ManagerStates.java | 16 ++-- 7 files changed, 74 insertions(+), 69 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java index d6cd32f..45c92a5 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java @@ -13,7 +13,6 @@ public class Indexer extends Subsystem { private Indexer() { super("Indexer", IndexerStates.OFF); - this.io = switch (GlobalConstants.ROBOT_MODE) { case SIM -> new IndexerIOSim(); case REAL -> new IndexerIOTalonFX(); diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java index 4d415c9..f546574 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerIOSim.java @@ -66,6 +66,9 @@ public void configurePID(PIDConstants constants) { @Override public boolean nextSensorTriggered() { - return Indexer.getInstance().getStateTime() > IndexerConstants.SIMULATED_INDEXING_TIME.in(Seconds); + return ( + Indexer.getInstance().getStateTime() > + IndexerConstants.SIMULATED_INDEXING_TIME.in(Seconds) + ); } } diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java index 03eab51..624ec2d 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java @@ -8,44 +8,45 @@ public class IndexerManager extends Subsystem { - private static IndexerManager instance; - private final Indexer indexer = Indexer.getInstance(); - - // Those who know - private IndexerManager() { - super("Indexer Manager", IndexerManagerStates.OFF); - } - - public static IndexerManager getInstance() { - if (instance == null) { - instance = new IndexerManager(); - } - 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() { - if (getState() == IndexerManagerStates.AUTONOMOUS) { - if (indexer.getState() == IndexerStates.OFF && Intake.getInstance().hasGamepiece()) { - indexer.setState(IndexerStates.INDEXING); - } else if (indexer.getState() == IndexerStates.INDEXING && indexer.nextSensorTriggered()) { - indexer.setState(IndexerStates.OFF); - } - } else { - indexer.setState(getState().getIndexerState()); - } - } + private static IndexerManager instance; + private final Indexer indexer = Indexer.getInstance(); + + // Those who know + private IndexerManager() { + super("Indexer Manager", IndexerManagerStates.OFF); + } + + public static IndexerManager getInstance() { + if (instance == null) { + instance = new IndexerManager(); + } + 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() { + if (getState() == IndexerManagerStates.AUTONOMOUS) { + if (indexer.getState() == IndexerStates.OFF && Intake.getInstance().hasGamepiece()) { + indexer.setState(IndexerStates.INDEXING); + } else if ( + indexer.getState() == IndexerStates.INDEXING && indexer.nextSensorTriggered() + ) { + indexer.setState(IndexerStates.OFF); + } + } else { + indexer.setState(getState().getIndexerState()); + } + } } diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java index 49e1883..d7dfe0b 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java @@ -4,24 +4,24 @@ import frc.robot.subsystems.Indexer.IndexerStates; public enum IndexerManagerStates implements SubsystemStates { - SCORING("SCORING", IndexerStates.SCORING), - OFF("OFF", IndexerStates.OFF), - AUTONOMOUS("Autonomous", IndexerStates.OFF); + SCORING("SCORING", IndexerStates.SCORING), + OFF("OFF", IndexerStates.OFF), + AUTONOMOUS("Autonomous", IndexerStates.OFF); - IndexerManagerStates(String stateString, IndexerStates indexerState) { - this.stateString = stateString; - this.indexerState = indexerState; - } + IndexerManagerStates(String stateString, IndexerStates indexerState) { + this.stateString = stateString; + this.indexerState = indexerState; + } - private String stateString; - private IndexerStates indexerState; + private String stateString; + private IndexerStates indexerState; - @Override - public String getStateString() { - return stateString; - } + @Override + public String getStateString() { + return stateString; + } - protected IndexerStates getIndexerState() { - return indexerState; - } + protected IndexerStates getIndexerState() { + return indexerState; + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 4258b84..0f8aee8 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -51,9 +51,9 @@ public static Intake getInstance() { return instance; } - public boolean hasGamepiece() { + public boolean hasGamepiece() { return io.hasGamepiece(); - } + } public double getStateTime() { return getStateTime(); diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java index 3f08119..2fd0074 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java @@ -108,7 +108,9 @@ public void setWheelSpeed(double wheelSpeedSetpoint) { } public boolean hasGamepiece() { - boolean gamepieceInIntake = Intake.getInstance().getStateTime() - lastIntookSimulatedGampieceTime > IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); + boolean gamepieceInIntake = + Intake.getInstance().getStateTime() - lastIntookSimulatedGampieceTime > + IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); if (gamepieceInIntake) { lastIntookSimulatedGampieceTime = Intake.getInstance().getStateTime(); } diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index 040e78d..b23127a 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -2,23 +2,23 @@ import frc.robot.pioneersLib.subsystem.SubsystemStates; import frc.robot.subsystems.Elevator.ElevatorStates; -import frc.robot.subsystems.Indexer.IndexerStates; import frc.robot.subsystems.Indexer.IndexerManager.IndexerManagerStates; +import frc.robot.subsystems.Indexer.IndexerStates; import frc.robot.subsystems.Intake.IntakeStates; public enum ManagerStates implements SubsystemStates { IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerManagerStates.OFF), INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerManagerStates.OFF), - OUTTAKING( - "OUTTAKING", - ElevatorStates.IDLE, - IntakeStates.OUTTAKING, - IndexerManagerStates.OFF - ), + OUTTAKING("OUTTAKING", ElevatorStates.IDLE, IntakeStates.OUTTAKING, IndexerManagerStates.OFF), GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerManagerStates.OFF), SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerManagerStates.SCORING), GOING_HIGH("GOING HIGH", ElevatorStates.HIGH, IntakeStates.INTAKING, IndexerManagerStates.OFF), - SCORING_HIGH("SCORING HIGH", ElevatorStates.HIGH, IntakeStates.IDLE, IndexerManagerStates.SCORING); + SCORING_HIGH( + "SCORING HIGH", + ElevatorStates.HIGH, + IntakeStates.IDLE, + IndexerManagerStates.SCORING + ); ManagerStates( String stateString, From d8e34312c06f106c4755a0a77c0f0eda25979e23 Mon Sep 17 00:00:00 2001 From: Otto Date: Fri, 3 Jan 2025 12:56:16 -0600 Subject: [PATCH 08/14] Ngl I really want a waffle maker --- .../java/frc/robot/subsystems/Indexer/Indexer.java | 1 - src/main/java/frc/robot/subsystems/Intake/Intake.java | 11 ----------- .../java/frc/robot/subsystems/Intake/IntakeIO.java | 1 - .../frc/robot/subsystems/Manager/ManagerStates.java | 1 - 4 files changed, 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java index d6cd32f..1e87ec1 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java @@ -2,7 +2,6 @@ import frc.robot.GlobalConstants; import frc.robot.pioneersLib.subsystem.Subsystem; -import frc.robot.subsystems.Intake.Intake; import org.littletonrobotics.junction.Logger; public class Indexer extends Subsystem { diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 4258b84..8c2aac6 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -1,9 +1,7 @@ package frc.robot.subsystems.Intake; -import static edu.wpi.first.units.Units.Seconds; import static frc.robot.GlobalConstants.ROBOT_MODE; -import frc.robot.GlobalConstants; import frc.robot.pioneersLib.subsystem.Subsystem; import org.littletonrobotics.junction.Logger; @@ -13,23 +11,14 @@ public class Intake extends Subsystem { private IntakeIOInputsAutoLogged inputs; private static Intake instance; - // Strictly for sim - private double lastIntookSimulatedGampieceTime; - private Intake(IntakeIO io) { super("Intake", IntakeStates.IDLE); this.io = io; inputs = new IntakeIOInputsAutoLogged(); - lastIntookSimulatedGampieceTime = 0.0; } @Override protected void runState() { - // ehhhh, yeah idk if this will work, ig its in the same loop as the reset so it should theoretically work - if (getStateTime() == 0) { - lastIntookSimulatedGampieceTime = 0.0; - } - io.setPivotSetpoint(getState().getPivotSetpoint()); io.setWheelSpeed(getState().getWheelSpeedSetpoint()); diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java index fbb09ff..118d683 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.Intake; -import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Default; import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index 040e78d..18e585f 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -2,7 +2,6 @@ import frc.robot.pioneersLib.subsystem.SubsystemStates; import frc.robot.subsystems.Elevator.ElevatorStates; -import frc.robot.subsystems.Indexer.IndexerStates; import frc.robot.subsystems.Indexer.IndexerManager.IndexerManagerStates; import frc.robot.subsystems.Intake.IntakeStates; From 81776d45a8a2e3d5c96a2b0529a60d7e7b13b3a3 Mon Sep 17 00:00:00 2001 From: Otto Date: Fri, 3 Jan 2025 13:22:22 -0600 Subject: [PATCH 09/14] Renaming --- .../IndexerManager/IndexerManager.java | 52 ------------------- .../IndexerManager/IndexerManagerStates.java | 27 ---------- .../PublicIndexerInterface.java | 37 +++++++++++++ .../PublicIndexerInterfaceStates.java | 21 ++++++++ .../frc/robot/subsystems/Manager/Manager.java | 14 ++--- .../subsystems/Manager/ManagerStates.java | 27 +++++----- 6 files changed, 78 insertions(+), 100 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java delete mode 100644 src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java create mode 100644 src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java create mode 100644 src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterfaceStates.java diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java deleted file mode 100644 index 624ec2d..0000000 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManager.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.robot.subsystems.Indexer.IndexerManager; - -import frc.robot.pioneersLib.subsystem.Subsystem; -import frc.robot.subsystems.Indexer.Indexer; -import frc.robot.subsystems.Indexer.IndexerConstants; -import frc.robot.subsystems.Indexer.IndexerStates; -import frc.robot.subsystems.Intake.Intake; - -public class IndexerManager extends Subsystem { - - private static IndexerManager instance; - private final Indexer indexer = Indexer.getInstance(); - - // Those who know - private IndexerManager() { - super("Indexer Manager", IndexerManagerStates.OFF); - } - - public static IndexerManager getInstance() { - if (instance == null) { - instance = new IndexerManager(); - } - 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() { - if (getState() == IndexerManagerStates.AUTONOMOUS) { - if (indexer.getState() == IndexerStates.OFF && Intake.getInstance().hasGamepiece()) { - indexer.setState(IndexerStates.INDEXING); - } else if ( - indexer.getState() == IndexerStates.INDEXING && indexer.nextSensorTriggered() - ) { - indexer.setState(IndexerStates.OFF); - } - } else { - indexer.setState(getState().getIndexerState()); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java deleted file mode 100644 index d7dfe0b..0000000 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerManagerStates.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.subsystems.Indexer.IndexerManager; - -import frc.robot.pioneersLib.subsystem.SubsystemStates; -import frc.robot.subsystems.Indexer.IndexerStates; - -public enum IndexerManagerStates implements SubsystemStates { - SCORING("SCORING", IndexerStates.SCORING), - OFF("OFF", IndexerStates.OFF), - AUTONOMOUS("Autonomous", IndexerStates.OFF); - - IndexerManagerStates(String stateString, IndexerStates indexerState) { - this.stateString = stateString; - this.indexerState = indexerState; - } - - private String stateString; - private IndexerStates indexerState; - - @Override - public String getStateString() { - return stateString; - } - - protected IndexerStates getIndexerState() { - return indexerState; - } -} diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java new file mode 100644 index 0000000..00da7a2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java @@ -0,0 +1,37 @@ +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 PublicIndexerInterface extends Subsystem { + + private static PublicIndexerInterface instance; + + private PublicIndexerInterface() { + super("Indexer Manager", PublicIndexerInterfaceStates.OFF); + } + + public static PublicIndexerInterface getInstance() { + if (instance == null) { + instance = new PublicIndexerInterface(); + } + 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() { + } +} diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterfaceStates.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterfaceStates.java new file mode 100644 index 0000000..bff77a7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterfaceStates.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 PublicIndexerInterfaceStates implements SubsystemStates { + SCORING("SCORING"), + OFF("OFF"), + AUTONOMOUS("Autonomous"); + + PublicIndexerInterfaceStates(String stateString) { + this.stateString = stateString; + } + + private String stateString; + + @Override + public String getStateString() { + return stateString; + } +} diff --git a/src/main/java/frc/robot/subsystems/Manager/Manager.java b/src/main/java/frc/robot/subsystems/Manager/Manager.java index 0d2bb21..30d567c 100644 --- a/src/main/java/frc/robot/subsystems/Manager/Manager.java +++ b/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.Indexer.IndexerManager.IndexerManager; +import frc.robot.subsystems.Indexer.IndexerManager.PublicIndexerInterface; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.Vision.Vision; import org.littletonrobotics.junction.Logger; @@ -22,7 +22,7 @@ public class Manager extends Subsystem { private final Elevator elevator = Elevator.getInstance(); private final CommandScheduler commandScheduler = CommandScheduler.getInstance(); private final Intake intake = Intake.getInstance(); - private final IndexerManager indexerManager = IndexerManager.getInstance(); + private final PublicIndexerInterface indexerInterface = PublicIndexerInterface.getInstance(); // Change to change the subsystem that gets tested (has runnable sysID tests) saftey ish private final Subsystem sysIdSubsystem = drive; @@ -87,7 +87,7 @@ private Manager() { () -> Controllers.DRIVER_CONTROLLER.getBButtonPressed() || Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || - indexerManager.isFull() + indexerInterface.isFull() ); addTrigger(ManagerStates.INTAKING, ManagerStates.OUTTAKING, () -> Controllers.DRIVER_CONTROLLER.getAButtonPressed() @@ -109,7 +109,7 @@ private Manager() { addTrigger( ManagerStates.SCORING_HIGH, ManagerStates.IDLE, - () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexerManager.isEmpty() + () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexerInterface.isEmpty() ); // from going mid @@ -119,7 +119,7 @@ private Manager() { addTrigger( ManagerStates.SCORING_MID, ManagerStates.IDLE, - () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexerManager.isEmpty() + () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexerInterface.isEmpty() ); // return to idle @@ -146,12 +146,12 @@ public void runState() { elevator.setState(getState().getElevatorState()); intake.setState(getState().getIntakeState()); - indexerManager.setState(getState().getIndexerManagerState()); + indexerInterface.setState(getState().getIndexerInterfaceState()); drive.periodic(); vision.periodic(); elevator.periodic(); intake.periodic(); - indexerManager.periodic(); + indexerInterface.periodic(); } } diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index b23127a..bfae2a7 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -2,40 +2,39 @@ import frc.robot.pioneersLib.subsystem.SubsystemStates; import frc.robot.subsystems.Elevator.ElevatorStates; -import frc.robot.subsystems.Indexer.IndexerManager.IndexerManagerStates; -import frc.robot.subsystems.Indexer.IndexerStates; +import frc.robot.subsystems.Indexer.IndexerManager.PublicIndexerInterfaceStates; import frc.robot.subsystems.Intake.IntakeStates; public enum ManagerStates implements SubsystemStates { - IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerManagerStates.OFF), - INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerManagerStates.OFF), - OUTTAKING("OUTTAKING", ElevatorStates.IDLE, IntakeStates.OUTTAKING, IndexerManagerStates.OFF), - GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerManagerStates.OFF), - SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerManagerStates.SCORING), - GOING_HIGH("GOING HIGH", ElevatorStates.HIGH, IntakeStates.INTAKING, IndexerManagerStates.OFF), + IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, PublicIndexerInterfaceStates.OFF), + INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, PublicIndexerInterfaceStates.OFF), + OUTTAKING("OUTTAKING", ElevatorStates.IDLE, IntakeStates.OUTTAKING, PublicIndexerInterfaceStates.OFF), + GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, PublicIndexerInterfaceStates.OFF), + SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, PublicIndexerInterfaceStates.SCORING), + GOING_HIGH("GOING HIGH", ElevatorStates.HIGH, IntakeStates.INTAKING, PublicIndexerInterfaceStates.OFF), SCORING_HIGH( "SCORING HIGH", ElevatorStates.HIGH, IntakeStates.IDLE, - IndexerManagerStates.SCORING + PublicIndexerInterfaceStates.SCORING ); ManagerStates( String stateString, ElevatorStates elevatorState, IntakeStates intakeState, - IndexerManagerStates indexerManagerState + PublicIndexerInterfaceStates indexerInterfaceState ) { this.stateString = stateString; this.elevatorState = elevatorState; this.intakeState = intakeState; - this.indexerManagerState = indexerManagerState; + this.indexerInterfaceState = indexerInterfaceState; } private String stateString; private IntakeStates intakeState; private ElevatorStates elevatorState; - private IndexerManagerStates indexerManagerState; + private PublicIndexerInterfaceStates indexerInterfaceState; @Override public String getStateString() { @@ -50,7 +49,7 @@ protected IntakeStates getIntakeState() { return intakeState; } - protected IndexerManagerStates getIndexerManagerState() { - return indexerManagerState; + protected PublicIndexerInterfaceStates getIndexerInterfaceState() { + return indexerInterfaceState; } } From 9026b3b72833f5452558b71f6f81115cf5423f31 Mon Sep 17 00:00:00 2001 From: github-actions Date: Fri, 3 Jan 2025 19:22:31 +0000 Subject: [PATCH 10/14] Apply Prettier format --- .../PublicIndexerInterface.java | 3 +- .../subsystems/Manager/ManagerStates.java | 28 ++++++++++++++++--- 2 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java index 00da7a2..19d0336 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java @@ -32,6 +32,5 @@ public boolean isFull() { } @Override - public void runState() { - } + public void runState() {} } diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index bfae2a7..8d640d6 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -7,11 +7,31 @@ public enum ManagerStates implements SubsystemStates { IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, PublicIndexerInterfaceStates.OFF), - INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, PublicIndexerInterfaceStates.OFF), - OUTTAKING("OUTTAKING", ElevatorStates.IDLE, IntakeStates.OUTTAKING, PublicIndexerInterfaceStates.OFF), + INTAKING( + "INTAKING", + ElevatorStates.IDLE, + IntakeStates.INTAKING, + PublicIndexerInterfaceStates.OFF + ), + OUTTAKING( + "OUTTAKING", + ElevatorStates.IDLE, + IntakeStates.OUTTAKING, + PublicIndexerInterfaceStates.OFF + ), GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, PublicIndexerInterfaceStates.OFF), - SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, PublicIndexerInterfaceStates.SCORING), - GOING_HIGH("GOING HIGH", ElevatorStates.HIGH, IntakeStates.INTAKING, PublicIndexerInterfaceStates.OFF), + SCORING_MID( + "SCORING MID", + ElevatorStates.MID, + IntakeStates.IDLE, + PublicIndexerInterfaceStates.SCORING + ), + GOING_HIGH( + "GOING HIGH", + ElevatorStates.HIGH, + IntakeStates.INTAKING, + PublicIndexerInterfaceStates.OFF + ), SCORING_HIGH( "SCORING HIGH", ElevatorStates.HIGH, From 9971ff3d81a0b496f75186b53414f6c2a2ca505b Mon Sep 17 00:00:00 2001 From: Otto Date: Fri, 3 Jan 2025 20:24:54 -0600 Subject: [PATCH 11/14] Boiled up a light drizzle --- .../frc/robot/subsystems/Indexer/Indexer.java | 21 +++++++++++++++++- ...erInterface.java => IndexerInterface.java} | 12 +++++----- ...tates.java => IndexerInterfaceStates.java} | 4 ++-- .../subsystems/Indexer/IndexerStates.java | 3 ++- .../frc/robot/subsystems/Manager/Manager.java | 4 ++-- .../subsystems/Manager/ManagerStates.java | 22 +++++++++---------- 6 files changed, 43 insertions(+), 23 deletions(-) rename src/main/java/frc/robot/subsystems/Indexer/IndexerManager/{PublicIndexerInterface.java => IndexerInterface.java} (64%) rename src/main/java/frc/robot/subsystems/Indexer/IndexerManager/{PublicIndexerInterfaceStates.java => IndexerInterfaceStates.java} (74%) diff --git a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java index c8c8bf3..40fed65 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java @@ -2,6 +2,10 @@ 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 { @@ -10,6 +14,7 @@ public class Indexer extends Subsystem { 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); this.io = switch (GlobalConstants.ROBOT_MODE) { @@ -19,7 +24,21 @@ private Indexer() { 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() { if (instance == null) { diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterface.java similarity index 64% rename from src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java rename to src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterface.java index 00da7a2..9087889 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterface.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterface.java @@ -4,17 +4,17 @@ import frc.robot.subsystems.Indexer.Indexer; import frc.robot.subsystems.Indexer.IndexerConstants; -public class PublicIndexerInterface extends Subsystem { +public class IndexerInterface extends Subsystem { - private static PublicIndexerInterface instance; + private static IndexerInterface instance; - private PublicIndexerInterface() { - super("Indexer Manager", PublicIndexerInterfaceStates.OFF); + private IndexerInterface() { + super("Indexer Manager", IndexerInterfaceStates.OFF); } - public static PublicIndexerInterface getInstance() { + public static IndexerInterface getInstance() { if (instance == null) { - instance = new PublicIndexerInterface(); + instance = new IndexerInterface(); } return instance; } diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterfaceStates.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterfaceStates.java similarity index 74% rename from src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterfaceStates.java rename to src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterfaceStates.java index bff77a7..28b6540 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/PublicIndexerInterfaceStates.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterfaceStates.java @@ -3,12 +3,12 @@ import frc.robot.pioneersLib.subsystem.SubsystemStates; import frc.robot.subsystems.Indexer.IndexerStates; -public enum PublicIndexerInterfaceStates implements SubsystemStates { +public enum IndexerInterfaceStates implements SubsystemStates { SCORING("SCORING"), OFF("OFF"), AUTONOMOUS("Autonomous"); - PublicIndexerInterfaceStates(String stateString) { + IndexerInterfaceStates(String stateString) { this.stateString = stateString; } diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java index 8089b46..9f5032d 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java @@ -4,7 +4,8 @@ public enum IndexerStates implements SubsystemStates { OFF("Off", IndexerConstants.OFF), - INDEXING("Indexing", IndexerConstants.ON), + AUTO_ON("Indexing", IndexerConstants.ON), + AUTO_OFF("Indexing", IndexerConstants.OFF), SCORING("Scoring", IndexerConstants.ON); private String stateString; diff --git a/src/main/java/frc/robot/subsystems/Manager/Manager.java b/src/main/java/frc/robot/subsystems/Manager/Manager.java index 30d567c..487a031 100644 --- a/src/main/java/frc/robot/subsystems/Manager/Manager.java +++ b/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.Indexer.IndexerManager.PublicIndexerInterface; +import frc.robot.subsystems.Indexer.IndexerManager.IndexerInterface; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.Vision.Vision; import org.littletonrobotics.junction.Logger; @@ -22,7 +22,7 @@ public class Manager extends Subsystem { private final Elevator elevator = Elevator.getInstance(); private final CommandScheduler commandScheduler = CommandScheduler.getInstance(); private final Intake intake = Intake.getInstance(); - private final PublicIndexerInterface indexerInterface = PublicIndexerInterface.getInstance(); + private final IndexerInterface indexerInterface = IndexerInterface.getInstance(); // Change to change the subsystem that gets tested (has runnable sysID tests) saftey ish private final Subsystem sysIdSubsystem = drive; diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index bfae2a7..a4c54fe 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -2,28 +2,28 @@ import frc.robot.pioneersLib.subsystem.SubsystemStates; import frc.robot.subsystems.Elevator.ElevatorStates; -import frc.robot.subsystems.Indexer.IndexerManager.PublicIndexerInterfaceStates; +import frc.robot.subsystems.Indexer.IndexerManager.IndexerInterfaceStates; import frc.robot.subsystems.Intake.IntakeStates; public enum ManagerStates implements SubsystemStates { - IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, PublicIndexerInterfaceStates.OFF), - INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, PublicIndexerInterfaceStates.OFF), - OUTTAKING("OUTTAKING", ElevatorStates.IDLE, IntakeStates.OUTTAKING, PublicIndexerInterfaceStates.OFF), - GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, PublicIndexerInterfaceStates.OFF), - SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, PublicIndexerInterfaceStates.SCORING), - GOING_HIGH("GOING HIGH", ElevatorStates.HIGH, IntakeStates.INTAKING, PublicIndexerInterfaceStates.OFF), + IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerInterfaceStates.OFF), + INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerInterfaceStates.OFF), + OUTTAKING("OUTTAKING", ElevatorStates.IDLE, IntakeStates.OUTTAKING, IndexerInterfaceStates.OFF), + GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerInterfaceStates.OFF), + SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerInterfaceStates.SCORING), + GOING_HIGH("GOING HIGH", ElevatorStates.HIGH, IntakeStates.INTAKING, IndexerInterfaceStates.OFF), SCORING_HIGH( "SCORING HIGH", ElevatorStates.HIGH, IntakeStates.IDLE, - PublicIndexerInterfaceStates.SCORING + IndexerInterfaceStates.SCORING ); ManagerStates( String stateString, ElevatorStates elevatorState, IntakeStates intakeState, - PublicIndexerInterfaceStates indexerInterfaceState + IndexerInterfaceStates indexerInterfaceState ) { this.stateString = stateString; this.elevatorState = elevatorState; @@ -34,7 +34,7 @@ public enum ManagerStates implements SubsystemStates { private String stateString; private IntakeStates intakeState; private ElevatorStates elevatorState; - private PublicIndexerInterfaceStates indexerInterfaceState; + private IndexerInterfaceStates indexerInterfaceState; @Override public String getStateString() { @@ -49,7 +49,7 @@ protected IntakeStates getIntakeState() { return intakeState; } - protected PublicIndexerInterfaceStates getIndexerInterfaceState() { + protected IndexerInterfaceStates getIndexerInterfaceState() { return indexerInterfaceState; } } From 570df6eed4f4c972b5a03582e975e0c5acb94cc3 Mon Sep 17 00:00:00 2001 From: Otto Date: Fri, 3 Jan 2025 20:25:54 -0600 Subject: [PATCH 12/14] Those who know --- src/main/java/frc/robot/subsystems/Manager/ManagerStates.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index a4c54fe..6d47b47 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -7,7 +7,7 @@ public enum ManagerStates implements SubsystemStates { IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerInterfaceStates.OFF), - INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerInterfaceStates.OFF), + INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerInterfaceStates.AUTONOMOUS), OUTTAKING("OUTTAKING", ElevatorStates.IDLE, IntakeStates.OUTTAKING, IndexerInterfaceStates.OFF), GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerInterfaceStates.OFF), SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerInterfaceStates.SCORING), From 1ceba5a4fe57f9c95a0564bf71a0bdb34c8576a9 Mon Sep 17 00:00:00 2001 From: github-actions Date: Sat, 4 Jan 2025 02:26:04 +0000 Subject: [PATCH 13/14] Apply Prettier format --- .../frc/robot/subsystems/Indexer/Indexer.java | 50 +++++++++++++++---- .../subsystems/Manager/ManagerStates.java | 21 ++++++-- 2 files changed, 58 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java index 40fed65..de77f9f 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java @@ -5,7 +5,6 @@ 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 { @@ -26,19 +25,50 @@ private Indexer() { this.inputs = new IndexerIOInputsAutoLogged(); // State for scoring - addTrigger(IndexerStates.OFF, IndexerStates.SCORING, () -> IndexerInterface.getInstance().getState() == IndexerInterfaceStates.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); - + 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()); - } + 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() { if (instance == null) { diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index 6d47b47..7de1f19 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -7,11 +7,26 @@ public enum ManagerStates implements SubsystemStates { IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerInterfaceStates.OFF), - INTAKING("INTAKING", ElevatorStates.IDLE, IntakeStates.INTAKING, IndexerInterfaceStates.AUTONOMOUS), + INTAKING( + "INTAKING", + ElevatorStates.IDLE, + IntakeStates.INTAKING, + IndexerInterfaceStates.AUTONOMOUS + ), OUTTAKING("OUTTAKING", ElevatorStates.IDLE, IntakeStates.OUTTAKING, IndexerInterfaceStates.OFF), GOING_MID("GOING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerInterfaceStates.OFF), - SCORING_MID("SCORING MID", ElevatorStates.MID, IntakeStates.IDLE, IndexerInterfaceStates.SCORING), - GOING_HIGH("GOING HIGH", ElevatorStates.HIGH, IntakeStates.INTAKING, IndexerInterfaceStates.OFF), + SCORING_MID( + "SCORING MID", + ElevatorStates.MID, + IntakeStates.IDLE, + IndexerInterfaceStates.SCORING + ), + GOING_HIGH( + "GOING HIGH", + ElevatorStates.HIGH, + IntakeStates.INTAKING, + IndexerInterfaceStates.OFF + ), SCORING_HIGH( "SCORING HIGH", ElevatorStates.HIGH, From 6f8cf50e8f391b1aa08b22deaa1b55eb331ac13e Mon Sep 17 00:00:00 2001 From: Otto Date: Sat, 4 Jan 2025 10:58:45 -0600 Subject: [PATCH 14/14] Cleanup --- .../frc/robot/pioneersLib/subsystem/Subsystem.java | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/main/java/frc/robot/pioneersLib/subsystem/Subsystem.java b/src/main/java/frc/robot/pioneersLib/subsystem/Subsystem.java index 23d49d8..f21fe4c 100644 --- a/src/main/java/frc/robot/pioneersLib/subsystem/Subsystem.java +++ b/src/main/java/frc/robot/pioneersLib/subsystem/Subsystem.java @@ -101,16 +101,6 @@ public void setState(StateType state) { this.state = state; } - // TODO: This is mad goofy - // For if you need to override set state - public void manualSetState(StateType state) { - this.state = state; - } - - public void resetStateTimer() { - stateTimer.reset(); - } - /** * Gets amount of time the state machine has been in the current state. *