diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8d39795..42c1b10 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -169,28 +169,13 @@ private void configureBindings() { .x() .onTrue(robotCommands.outtakeCommand()) .onFalse(robotCommands.stowCommand()); - hardware - .operatorController - .a() - .onTrue(robotCommands.stowCommand()); - hardware - .operatorController - .povUp() - .onTrue(robotCommands.climbUpCommand()); - hardware - .operatorController - .povDown() - .onTrue(robotCommands.climbDownCommand()); + hardware.operatorController.a().onTrue(robotCommands.stowCommand()); + hardware.operatorController.povUp().onTrue(robotCommands.climbUpCommand()); + hardware.operatorController.povDown().onTrue(robotCommands.climbDownCommand()); hardware .operatorController .povLeft() .onTrue(robotCommands.unjamCommand()) .onFalse(robotCommands.stowCommand()); - - - - - - } } diff --git a/src/main/java/frc/robot/config/CompConfig.java b/src/main/java/frc/robot/config/CompConfig.java index da3f0bb..d1debbe 100644 --- a/src/main/java/frc/robot/config/CompConfig.java +++ b/src/main/java/frc/robot/config/CompConfig.java @@ -66,7 +66,6 @@ class CompConfig { 999, CANIVORE_NAME, 999, - 999, new TalonFXConfiguration() .withClosedLoopRamps(CLOSED_LOOP_RAMP) .withOpenLoopRamps(OPEN_LOOP_RAMP) @@ -77,8 +76,7 @@ class CompConfig { centeringMotor -> { centeringMotor.setSmartCurrentLimit(20); centeringMotor.burnFlash(); - }, - new Debouncer(3.0 * 0.02)), + }), new ArmConfig( CANIVORE_NAME, 999, diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java index 212c28b..2e2d96b 100644 --- a/src/main/java/frc/robot/config/RobotConfig.java +++ b/src/main/java/frc/robot/config/RobotConfig.java @@ -39,10 +39,8 @@ public record IntakeConfig( int mainMotorID, String mainMotorCanBusName, int centeringMotorID, - int sensorID, TalonFXConfiguration mainMotorConfig, - Consumer centeringMotorConfig, - Debouncer debouncer) {} + Consumer centeringMotorConfig) {} public record ArmConfig( String canBusName, diff --git a/src/main/java/frc/robot/intake/IntakeSubsystem.java b/src/main/java/frc/robot/intake/IntakeSubsystem.java index daeed9d..b739bb4 100644 --- a/src/main/java/frc/robot/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/intake/IntakeSubsystem.java @@ -11,8 +11,6 @@ public class IntakeSubsystem extends StateMachine { private final TalonFX mainMotor; private final CANSparkMax centeringMotor; - private boolean sensorHasNote = false; - private boolean debouncedSensorHasNote = false; private final Debouncer debouncer = RobotConfig.get().intake().debouncer(); public IntakeSubsystem(TalonFX mainMotor, CANSparkMax centeringMotor) { @@ -29,20 +27,11 @@ public void setState(IntakeState newState) { setStateFromRequest(newState); } - @Override - protected void collectInputs() { - debouncedSensorHasNote = debouncer.calculate(sensorHasNote); - } - @Override protected IntakeState getNextState(IntakeState currentState) { return currentState; } - public boolean hasNote() { - return debouncedSensorHasNote; - } - @Override protected void afterTransition(IntakeState newState) { switch (newState) { @@ -65,7 +54,5 @@ public void robotPeriodic() { DogLog.log("Intake/StatorCurrent", mainMotor.getStatorCurrent().getValueAsDouble()); DogLog.log("Intake/SupplyCurrent", mainMotor.getSupplyCurrent().getValueAsDouble()); DogLog.log("Intake/AppliedVoltage", mainMotor.getMotorVoltage().getValueAsDouble()); - DogLog.log("Intake/RawSensor", sensorHasNote); - DogLog.log("Intake/DebouncedSensor", hasNote()); } } diff --git a/src/main/java/frc/robot/queuer/QueuerState.java b/src/main/java/frc/robot/queuer/QueuerState.java index 99cbfcd..3ff4764 100644 --- a/src/main/java/frc/robot/queuer/QueuerState.java +++ b/src/main/java/frc/robot/queuer/QueuerState.java @@ -1,8 +1,7 @@ package frc.robot.queuer; public enum QueuerState { - IDLE_WITH_GP, - IDLE_NO_GP, + IDLE, SHOOTING, INTAKING, OUTTAKING; diff --git a/src/main/java/frc/robot/queuer/QueuerSubsystem.java b/src/main/java/frc/robot/queuer/QueuerSubsystem.java index 0d469b8..e968e22 100644 --- a/src/main/java/frc/robot/queuer/QueuerSubsystem.java +++ b/src/main/java/frc/robot/queuer/QueuerSubsystem.java @@ -16,7 +16,7 @@ public class QueuerSubsystem extends StateMachine { private final Debouncer debouncer = RobotConfig.get().queuer().debouncer(); public QueuerSubsystem(TalonFX motor, DigitalInput sensor) { - super(SubsystemPriority.QUEUER, QueuerState.IDLE_NO_GP); + super(SubsystemPriority.QUEUER, queuer.setState(QueuerState.IDLE)); this.sensor = sensor; this.motor = motor; @@ -47,8 +47,7 @@ public boolean hasNote() { @Override protected void afterTransition(QueuerState newState) { switch (newState) { - case IDLE_NO_GP -> motor.disable(); - case IDLE_WITH_GP -> motor.disable(); + case IDLE -> motor.disable(); case SHOOTING -> motor.setVoltage(0); // probably like 12 case INTAKING -> motor.setVoltage(0); // probably like 4 case OUTTAKING -> motor.setVoltage(0); // proabably like -4 diff --git a/src/main/java/frc/robot/robot_manager/RobotCommands.java b/src/main/java/frc/robot/robot_manager/RobotCommands.java index 17c1051..d942f71 100644 --- a/src/main/java/frc/robot/robot_manager/RobotCommands.java +++ b/src/main/java/frc/robot/robot_manager/RobotCommands.java @@ -89,15 +89,18 @@ public Command subwooferCommand() { return Commands.runOnce(robot::prepareSubwooferRequest, requirements) .andThen(robot.waitForState(RobotState.IDLE_NO_GP)); } - public Command climbUpCommand(){ + + public Command climbUpCommand() { return Commands.runOnce(robot::nextClimbStateRequest, requirements) .andThen(robot.waitForState(RobotState.CLIMBING_2_HANGING)); } - public Command climbDownCommand(){ + + public Command climbDownCommand() { return Commands.runOnce(robot::previousClimbStateRequest, requirements) .andThen(robot.waitForState(RobotState.CLIMBING_1_LINEUP)); } - public Command unjamCommand(){ + + public Command unjamCommand() { return Commands.runOnce(robot::unjamRequest, requirements) .andThen(robot.waitForState(RobotState.IDLE_NO_GP)); } diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index d04a3bf..df7c62e 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -76,7 +76,7 @@ protected RobotState getNextState(RobotState currentState) { shooter.atGoal() && arm.atGoal() ? RobotState.SUBWOOFER_SCORING : currentState; case UNJAM -> currentState; case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState; - case OUTTAKING -> queuer.hasNote() || intake.hasNote() ? currentState : RobotState.IDLE_NO_GP; + case OUTTAKING -> queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP; }; } @@ -87,7 +87,7 @@ protected void afterTransition(RobotState newState) { arm.setState(ArmState.SUBWOOFER_SHOT); shooter.setState(ShooterState.SUBWOOFER_SHOT); intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE); } case SUBWOOFER_SCORING -> { arm.setState(ArmState.SUBWOOFER_SHOT); @@ -99,7 +99,7 @@ protected void afterTransition(RobotState newState) { arm.setState(ArmState.SPEAKER_SHOT); shooter.setState(ShooterState.SPEAKER_SHOT); intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE); } case SPEAKER_SCORING -> { arm.setState(ArmState.SPEAKER_SHOT); @@ -111,7 +111,7 @@ protected void afterTransition(RobotState newState) { arm.setState(ArmState.AMP); shooter.setState(ShooterState.AMP); intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE); } case AMP_SCORING -> { arm.setState(ArmState.AMP); @@ -123,7 +123,7 @@ protected void afterTransition(RobotState newState) { arm.setState(ArmState.FEEDING); shooter.setState(ShooterState.FEEDING); intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE); } case FEEDING_SHOOTING -> { arm.setState(ArmState.FEEDING); @@ -135,7 +135,7 @@ protected void afterTransition(RobotState newState) { arm.setState(ArmState.PASS); shooter.setState(ShooterState.PASS); intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE); } case PASS_SHOOTING -> { arm.setState(ArmState.PASS); @@ -165,25 +165,25 @@ protected void afterTransition(RobotState newState) { arm.setState(ArmState.CLIMBING_1_LINEUP); shooter.setState(ShooterState.IDLE_STOPPED); intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE_NO_GP); + queuer.setState(queuer.setState(QueuerState.IDLE)); } case CLIMBING_2_HANGING -> { arm.setState(ArmState.CLIMBING_2_HANGING); shooter.setState(ShooterState.IDLE_STOPPED); intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE_NO_GP); + queuer.setState(queuer.setState(QueuerState.IDLE)); } case IDLE_NO_GP -> { arm.setState(ArmState.IDLE); shooter.setState(ShooterState.IDLE_STOPPED); intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE_NO_GP); + queuer.setState(queuer.setState(QueuerState.IDLE)); } case IDLE_WITH_GP -> { arm.setState(ArmState.IDLE); shooter.setState(ShooterState.IDLE_WARMUP); intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE_WITH_GP); + queuer.setState(QueuerState.IDLE); } } } @@ -343,11 +343,7 @@ public void prepareFeedRequest() { public void stopShootingRequest() { // If we are actively taking a shot, ignore the request to avoid messing up shooting switch (getState()) { - case SPEAKER_SCORING, - SUBWOOFER_SCORING, - AMP_SCORING, - FEEDING_SHOOTING, - PASS_SHOOTING -> {} + case SPEAKER_SCORING, SUBWOOFER_SCORING, AMP_SCORING, FEEDING_SHOOTING, PASS_SHOOTING -> {} default -> setStateFromRequest(RobotState.IDLE_WITH_GP); } }