diff --git a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java index b357704..de77f9f 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer/Indexer.java @@ -2,6 +2,9 @@ import frc.robot.GlobalConstants; import frc.robot.pioneersLib.subsystem.Subsystem; +import frc.robot.subsystems.Indexer.IndexerManager.IndexerInterface; +import frc.robot.subsystems.Indexer.IndexerManager.IndexerInterfaceStates; +import frc.robot.subsystems.Intake.Intake; import org.littletonrobotics.junction.Logger; public class Indexer extends Subsystem { @@ -10,13 +13,9 @@ 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); - addTrigger(IndexerStates.AUTONOMOUS_OFF, IndexerStates.AUTONOMOUS_ON, () -> - io.nextSensorTriggered() - ); - addTrigger(IndexerStates.AUTONOMOUS_ON, IndexerStates.AUTONOMOUS_OFF, () -> true); //TODO: actually get intake beam break - this.io = switch (GlobalConstants.ROBOT_MODE) { case SIM -> new IndexerIOSim(); case REAL -> new IndexerIOTalonFX(); @@ -24,6 +23,51 @@ 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() { @@ -45,8 +89,6 @@ public void stop() { public void periodic() { super.periodic(); - //TODO: Add output managing - Logger.processInputs("Indexer", inputs); io.updateInputs(inputs); } @@ -55,7 +97,11 @@ public int getNumberOfPieces() { return io.getNumberOfPieces(); } - public boolean isEmpty() { - return io.getNumberOfPieces() == 0; + public boolean nextSensorTriggered() { + return io.nextSensorTriggered(); + } + + public double getStateTime() { + return getStateTime(); } } 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..f546574 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; @@ -62,7 +64,11 @@ public void configurePID(PIDConstants constants) { speedController = new PIDController(constants.kP, constants.kI, constants.kD); } - public boolean[] getBeamBreakArray() { - return beamBreakArray; + @Override + public boolean nextSensorTriggered() { + return ( + Indexer.getInstance().getStateTime() > + IndexerConstants.SIMULATED_INDEXING_TIME.in(Seconds) + ); } } 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/IndexerInterface.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterface.java new file mode 100644 index 0000000..bcb63da --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterface.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems.Indexer.IndexerManager; + +import frc.robot.pioneersLib.subsystem.Subsystem; +import frc.robot.subsystems.Indexer.Indexer; +import frc.robot.subsystems.Indexer.IndexerConstants; + +public class IndexerInterface extends Subsystem { + + private static IndexerInterface instance; + + private IndexerInterface() { + super("Indexer Manager", IndexerInterfaceStates.OFF); + } + + public static IndexerInterface getInstance() { + if (instance == null) { + instance = new IndexerInterface(); + } + return instance; + } + + public int getNumberOfPieces() { + return Indexer.getInstance().getNumberOfPieces(); + } + + public boolean isEmpty() { + return Indexer.getInstance().getNumberOfPieces() == 0; + } + + public boolean isFull() { + return Indexer.getInstance().getNumberOfPieces() >= IndexerConstants.MAX_GAME_PIECES; + } + + @Override + public void runState() {} +} diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterfaceStates.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterfaceStates.java new file mode 100644 index 0000000..28b6540 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerManager/IndexerInterfaceStates.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.Indexer.IndexerManager; + +import frc.robot.pioneersLib.subsystem.SubsystemStates; +import frc.robot.subsystems.Indexer.IndexerStates; + +public enum IndexerInterfaceStates implements SubsystemStates { + SCORING("SCORING"), + OFF("OFF"), + AUTONOMOUS("Autonomous"); + + IndexerInterfaceStates(String stateString) { + this.stateString = stateString; + } + + private String stateString; + + @Override + public String getStateString() { + return stateString; + } +} diff --git a/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java b/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java index 806439b..9f5032d 100644 --- a/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java +++ b/src/main/java/frc/robot/subsystems/Indexer/IndexerStates.java @@ -4,8 +4,8 @@ public enum IndexerStates implements SubsystemStates { OFF("Off", IndexerConstants.OFF), - AUTONOMOUS_ON("Autonomous On", IndexerConstants.ON), - AUTONOMOUS_OFF("Autonomous Off", IndexerConstants.OFF), + AUTO_ON("Indexing", IndexerConstants.ON), + AUTO_OFF("Indexing", IndexerConstants.OFF), SCORING("Scoring", IndexerConstants.ON); private String stateString; diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 25a3910..4c10f63 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -7,8 +7,8 @@ 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 +39,12 @@ public static Intake getInstance() { } return instance; } + + public boolean hasGamepiece() { + return io.hasGamepiece(); + } + + public double getStateTime() { + return getStateTime(); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java index ec6f1c3..7241c26 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,15 @@ public static final class Sim { public static final MomentOfInertia WHEEL_MOTOR_MOI = KilogramSquareMeters.of(1); // random value public static final double WHEEL_MOTOR_GEARING = 3; public static final PIDConstants WHEEL_PID_CONSTANTS = new PIDConstants(0.0012, 0, 0); + + public static final Time SIMULATED_INTAKING_TIME = Seconds.of(1); } public static final class Real { public static final int WHEEL_MOTOR_CANID = 10; public static final int PIVOT_MOTOR_CANID = 11; + public static final int BEAM_BREAK_DIO_PORT = 10; public static final PIDConstants PIVOT_PID_CONSTANTS = new PIDConstants(3, 0, 0); public static final PIDConstants WHEEL_PID_CONSTANTS = new PIDConstants(0.0012, 0, 0); } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java index 88a3e69..118d683 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java @@ -15,9 +15,13 @@ public static class IntakeIOInputs { public double wheelSpeedSetpoint; } - public void updateInputs(IntakeIOInputs input); + public default void updateInputs(IntakeIOInputs input) {} - public void setPivotSetpoint(double pivotSetpoint); + public default void setPivotSetpoint(double pivotSetpoint) {} - public void setWheelSpeed(double wheelSpeed); + public default void setWheelSpeed(double wheelSpeed) {} + + public default boolean hasGamepiece() { + return false; + } } 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/Intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java index 5b751a7..2fd0074 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,14 @@ public void setWheelSpeed(double wheelSpeedSetpoint) { ) ); } + + public boolean hasGamepiece() { + boolean gamepieceInIntake = + Intake.getInstance().getStateTime() - lastIntookSimulatedGampieceTime > + IntakeConstants.Sim.SIMULATED_INTAKING_TIME.in(Seconds); + if (gamepieceInIntake) { + lastIntookSimulatedGampieceTime = Intake.getInstance().getStateTime(); + } + return gamepieceInIntake; + } } diff --git a/src/main/java/frc/robot/subsystems/Manager/Manager.java b/src/main/java/frc/robot/subsystems/Manager/Manager.java index 88e58c8..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.Indexer; +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 Indexer indexer = Indexer.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; @@ -57,6 +57,7 @@ private Manager() { () -> TEST_CONTROLLER.getYButtonPressed() ); } + // from idle addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () -> Controllers.DRIVER_CONTROLLER.getBButtonPressed() @@ -86,7 +87,7 @@ private Manager() { () -> Controllers.DRIVER_CONTROLLER.getBButtonPressed() || Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || - indexer.getNumberOfPieces() == MAX_PIECES + indexerInterface.isFull() ); addTrigger(ManagerStates.INTAKING, ManagerStates.OUTTAKING, () -> Controllers.DRIVER_CONTROLLER.getAButtonPressed() @@ -108,7 +109,7 @@ private Manager() { addTrigger( ManagerStates.SCORING_HIGH, ManagerStates.IDLE, - () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexer.isEmpty() + () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexerInterface.isEmpty() ); // from going mid @@ -118,7 +119,7 @@ private Manager() { addTrigger( ManagerStates.SCORING_MID, ManagerStates.IDLE, - () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexer.isEmpty() + () -> Controllers.OPERATOR_CONTROLLER.getAButtonPressed() || indexerInterface.isEmpty() ); // return to idle @@ -145,12 +146,12 @@ public void runState() { elevator.setState(getState().getElevatorState()); intake.setState(getState().getIntakeState()); - indexer.setState(getState().getIndexerState()); + indexerInterface.setState(getState().getIndexerInterfaceState()); drive.periodic(); vision.periodic(); elevator.periodic(); intake.periodic(); - indexer.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 225b828..7de1f19 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -2,39 +2,54 @@ import frc.robot.pioneersLib.subsystem.SubsystemStates; import frc.robot.subsystems.Elevator.ElevatorStates; -import frc.robot.subsystems.Indexer.IndexerStates; +import frc.robot.subsystems.Indexer.IndexerManager.IndexerInterfaceStates; 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_ON), - OUTTAKING( - "OUTTAKING", + IDLE("IDLE", ElevatorStates.IDLE, IntakeStates.IDLE, IndexerInterfaceStates.OFF), + INTAKING( + "INTAKING", ElevatorStates.IDLE, - IntakeStates.OUTTAKING, - IndexerStates.AUTONOMOUS_ON + IntakeStates.INTAKING, + IndexerInterfaceStates.AUTONOMOUS ), - 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); + 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, + IndexerInterfaceStates.SCORING + ); ManagerStates( String stateString, ElevatorStates elevatorState, IntakeStates intakeState, - IndexerStates indexerState + IndexerInterfaceStates indexerInterfaceState ) { this.stateString = stateString; this.elevatorState = elevatorState; this.intakeState = intakeState; - this.indexerState = indexerState; + this.indexerInterfaceState = indexerInterfaceState; } private String stateString; private IntakeStates intakeState; private ElevatorStates elevatorState; - private IndexerStates indexerState; + private IndexerInterfaceStates indexerInterfaceState; @Override public String getStateString() { @@ -49,7 +64,7 @@ protected IntakeStates getIntakeState() { return intakeState; } - protected IndexerStates getIndexerState() { - return indexerState; + protected IndexerInterfaceStates getIndexerInterfaceState() { + return indexerInterfaceState; } }