Skip to content

Commit

Permalink
Wow this code is so clean
Browse files Browse the repository at this point in the history
  • Loading branch information
@GreenTomato5
GreenTomato5 committed Oct 1, 2024
1 parent 040325e commit a3e5b8b
Showing 1 changed file with 117 additions and 121 deletions.
238 changes: 117 additions & 121 deletions src/main/java/frc/robot/subsystems/manager/Manager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,16 @@ public class Manager extends Subsystem<ManagerStates> {
private SendableChooser<Boolean> useVision;
private SendableChooser<Boolean> useHeadingCorrection;

private Boolean useBeamBreaksVal;
private Boolean useAutoAlignVal;
private Boolean driverShooterAfterSpinningVal;
private Boolean useClimbersVal;
private Boolean useVisionVal;
private Boolean useHeadingCorrectionVal;

private final int SENDABLE_CHECK_INTERVAL = 50;
private int loopCounter;

public Manager() {
super("Manager", ManagerStates.IDLE);
NoteSimulator.setDrive(driveSubsystem);
Expand Down Expand Up @@ -84,39 +94,46 @@ public Manager() {
shooterSubsystem = new Shooter(new ShooterIOTalonFX());
climberSubsystem = new Climber(new ClimberIOSparkMax());
driveSubsystem = new Drive(
new GyroIONavx2(),
new ModuleIOHybrid(0),
new ModuleIOHybrid(1),
new ModuleIOHybrid(2),
new ModuleIOHybrid(3)
);
new GyroIONavx2(),
new ModuleIOHybrid(0),
new ModuleIOHybrid(1),
new ModuleIOHybrid(2),
new ModuleIOHybrid(3));
useVision.setDefaultOption("On", true);
break;
case REPLAY:
intakeSubsystem = new Intake(new IntakeIO() {});
ampBarSubsystem = new AmpBar(new AmpBarIO() {});
shooterSubsystem = new Shooter(new ShooterIO() {});
climberSubsystem = new Climber(new ClimberIO() {});
intakeSubsystem = new Intake(new IntakeIO() {
});
ampBarSubsystem = new AmpBar(new AmpBarIO() {
});
shooterSubsystem = new Shooter(new ShooterIO() {
});
climberSubsystem = new Climber(new ClimberIO() {
});
driveSubsystem = new Drive(
new GyroIO() {},
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {},
new ModuleIO() {}
);
new GyroIO() {
},
new ModuleIO() {
},
new ModuleIO() {
},
new ModuleIO() {
},
new ModuleIO() {
});
break;
case SIM:
intakeSubsystem = new Intake(new IntakeIOSim());
ampBarSubsystem = new AmpBar(new AmpBarIOSim());
shooterSubsystem = new Shooter(new ShooterIOSim());
climberSubsystem = new Climber(new ClimberIOSim());
driveSubsystem = new Drive(
new GyroIO() {},
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim()
);
new GyroIO() {
},
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim());
useVision.setDefaultOption("Off", false);
break;
default:
Expand All @@ -127,101 +144,69 @@ public Manager() {

NoteSimulator.setDrive(driveSubsystem);

/* Generally each action has a specific button (Intaking, Shooting, etc.)
/*
* Generally each action has a specific button (Intaking, Shooting, etc.)
* In intermediary states X will return to idle
* Specific button will transition through the states
* Some transitions are automatic with timers or sensors */
* Some transitions are automatic with timers or sensors
*/

// Intaking (B)
addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () ->
Constants.controller.getBButtonPressed()
);
addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () -> Constants.controller.getBButtonPressed());
addTrigger(
ManagerStates.INTAKING,
ManagerStates.IDLE,
() -> intakeSubsystem.noteDetected() && intakeSubsystem.nearSetPoint() && (useBeamBreaks.getSelected() == null ? true : useBeamBreaks.getSelected()) || Constants.controller.getBButtonPressed()
);
ManagerStates.INTAKING,
ManagerStates.IDLE,
() -> intakeSubsystem.noteDetected() && intakeSubsystem.nearSetPoint() && useBeamBreaksVal
|| Constants.controller.getBButtonPressed());

// Amping (Y)
addTrigger(ManagerStates.IDLE, ManagerStates.STAGING_AMP, () ->
Constants.controller.getYButtonPressed()
);
addTrigger(ManagerStates.STAGING_AMP, ManagerStates.FEED_AMP, () ->
ampBarSubsystem.atSetPoint()
);
addTrigger(ManagerStates.IDLE, ManagerStates.STAGING_AMP, () -> Constants.controller.getYButtonPressed());
addTrigger(ManagerStates.STAGING_AMP, ManagerStates.FEED_AMP, () -> ampBarSubsystem.atSetPoint());
addTrigger(
ManagerStates.FEED_AMP,
ManagerStates.AMP_HOLDING_NOTE,
() ->
(ampBarSubsystem.noteDetected() &&
(useBeamBreaks.getSelected() == null ? true : useBeamBreaks.getSelected())) ||
Constants.controller.getYButtonPressed()
);
addTrigger(ManagerStates.AMP_HOLDING_NOTE, ManagerStates.SCORE_AMP, () ->
Constants.controller.getYButtonPressed()
);
addTrigger(ManagerStates.SCORE_AMP, ManagerStates.IDLE, () ->
(getStateTime() > Constants.AmpBar.TIME_FOR_SCORING)
);
addTrigger(ManagerStates.SCORE_AMP, ManagerStates.IDLE, () ->
Constants.controller.getYButtonPressed()
);
addTrigger(ManagerStates.FEED_AMP, ManagerStates.IDLE, () ->
Constants.controller.getXButtonPressed()
);
ManagerStates.FEED_AMP,
ManagerStates.AMP_HOLDING_NOTE,
() -> ampBarSubsystem.noteDetected() && useBeamBreaksVal ||
Constants.controller.getYButtonPressed());
addTrigger(ManagerStates.AMP_HOLDING_NOTE, ManagerStates.SCORE_AMP,
() -> Constants.controller.getYButtonPressed());
addTrigger(ManagerStates.SCORE_AMP, ManagerStates.IDLE,
() -> (getStateTime() > Constants.AmpBar.TIME_FOR_SCORING));
addTrigger(ManagerStates.SCORE_AMP, ManagerStates.IDLE, () -> Constants.controller.getYButtonPressed());
addTrigger(ManagerStates.FEED_AMP, ManagerStates.IDLE, () -> Constants.controller.getXButtonPressed());

// Shooting (A)
addTrigger(ManagerStates.IDLE, ManagerStates.SPINNING_UP, () ->
Constants.controller.getAButtonPressed()
);
addTrigger(ManagerStates.IDLE, ManagerStates.OPERATOR_SPINNING_UP, () ->
Constants.operatorController.getAButtonPressed()
);
addTrigger(ManagerStates.IDLE, ManagerStates.SPINNING_UP, () -> Constants.controller.getAButtonPressed());
addTrigger(ManagerStates.IDLE, ManagerStates.OPERATOR_SPINNING_UP,
() -> Constants.operatorController.getAButtonPressed());
addTrigger(
ManagerStates.SPINNING_UP,
ManagerStates.SHOOTING,
() ->
((driverShooterAfterSpinning.getSelected() == null
? true
: driverShooterAfterSpinning.getSelected()) &&
shooterSubsystem.nearSpeedPoint()) ||
Constants.controller.getAButtonPressed()
);
addTrigger(ManagerStates.SPINNING_UP, ManagerStates.IDLE, () ->
Constants.controller.getXButtonPressed()
);
addTrigger(ManagerStates.OPERATOR_SPINNING_UP, ManagerStates.SHOOTING, () ->
Constants.controller.getAButtonPressed()
);
addTrigger(ManagerStates.SHOOTING, ManagerStates.IDLE, () ->
Constants.controller.getAButtonPressed()
);
ManagerStates.SPINNING_UP,
ManagerStates.SHOOTING,
() -> driverShooterAfterSpinningVal ||
Constants.controller.getAButtonPressed());
addTrigger(ManagerStates.SPINNING_UP, ManagerStates.IDLE, () -> Constants.controller.getXButtonPressed());
addTrigger(ManagerStates.OPERATOR_SPINNING_UP, ManagerStates.SHOOTING,
() -> Constants.controller.getAButtonPressed());
addTrigger(ManagerStates.SHOOTING, ManagerStates.IDLE, () -> Constants.controller.getAButtonPressed());

// Climbing (POV)
addTrigger(
ManagerStates.IDLE,
ManagerStates.STAGING_FOR_CLIMB,
() ->
Constants.controller.getPOV() == 0 &&
(useClimbers.getSelected() == null ? true : useClimbers.getSelected())
);
addTrigger(ManagerStates.STAGING_FOR_CLIMB, ManagerStates.CLIMBING, () ->
ampBarSubsystem.nearSetPoints()
);
ManagerStates.IDLE,
ManagerStates.STAGING_FOR_CLIMB,
() -> Constants.controller.getPOV() == 0 && useClimbersVal);
addTrigger(ManagerStates.STAGING_FOR_CLIMB, ManagerStates.CLIMBING, () -> ampBarSubsystem.nearSetPoints());
addTrigger(
ManagerStates.CLIMBING,
ManagerStates.CLIMBED,
() -> Constants.controller.getPOV() == 180
);
addTrigger(ManagerStates.CLIMBED, ManagerStates.IDLE, () ->
Constants.controller.getXButtonPressed()
);
ManagerStates.CLIMBING,
ManagerStates.CLIMBED,
() -> Constants.controller.getPOV() == 180);
addTrigger(ManagerStates.CLIMBED, ManagerStates.IDLE, () -> Constants.controller.getXButtonPressed());
}

@Override
public void periodic() {
super.periodic();

loopCounter += 1;

intakeSubsystem.setState(getState().getIntakeState());
ampBarSubsystem.setState(getState().getAmpBarState());
shooterSubsystem.setState(getState().getShooterState());
Expand All @@ -232,37 +217,48 @@ public void periodic() {
shooterSubsystem.periodic();
driveSubsystem.periodic();

// if (useVision.getSelected() == null ? true : useVision.getSelected()) vision.periodic();
if (
useAutoAlign.getSelected() == null ? true : useAutoAlign.getSelected()
) autoAlignSubsystem.periodic();
if (useClimbers.getSelected() == null ? true : useClimbers.getSelected()) {
climberSubsystem.periodic();
climberSubsystem.setState(getState().getClimberState());
// Set Sendable Stuff:
if (loopCounter % SENDABLE_CHECK_INTERVAL == 0) {
useBeamBreaksVal = useBeamBreaks.getSelected() == null ? true : useBeamBreaks.getSelected();
useAutoAlignVal = useAutoAlign.getSelected() == null ? true : useAutoAlign.getSelected();
driverShooterAfterSpinningVal = driverShooterAfterSpinning.getSelected() == null ? true
: driverShooterAfterSpinning.getSelected();
useClimbersVal = useClimbers.getSelected() == null ? true : useClimbers.getSelected();
useVisionVal = useVision.getSelected() == null ? true : useVision.getSelected();
useHeadingCorrectionVal = useHeadingCorrection.getSelected() == null ? true
: useHeadingCorrection.getSelected();
}

// AA
if (autoAlignSubsystem.nearTargetPoint()) {
switch (autoAlignSubsystem.getCachedState()) {
case AMP:
setState(ManagerStates.STAGING_AMP);
break;
case AMP_SPEAKER:
setState(ManagerStates.SPINNING_UP);
break;
case SOURCE_SPEAKER:
setState(ManagerStates.SPINNING_UP);
break;
case OFF:
break;
default:
break;
if (useAutoAlignVal) {
autoAlignSubsystem.periodic();
if (autoAlignSubsystem.nearTargetPoint()) {
switch (autoAlignSubsystem.getCachedState()) {
case AMP:
setState(ManagerStates.STAGING_AMP);
break;
case AMP_SPEAKER:
setState(ManagerStates.SPINNING_UP);
break;
case SOURCE_SPEAKER:
setState(ManagerStates.SPINNING_UP);
break;
case OFF:
break;
default:
break;
}
}
}

driveSubsystem.toggleHeadingCorrection(
useHeadingCorrection.getSelected() == null ? true : useHeadingCorrection.getSelected()
);
if (useClimbersVal) {
climberSubsystem.periodic();
climberSubsystem.setState(getState().getClimberState());
}

// if (useVisionVal) vision.periodic();

driveSubsystem.toggleHeadingCorrection(useHeadingCorrectionVal);

// Cancel all actions regardless of whats happening
if (Constants.operatorController.getXButtonPressed()) {
Expand Down

0 comments on commit a3e5b8b

Please sign in to comment.