Skip to content

Commit e4fb63b

Browse files
Wu-Fan-529Wu-Fan-529
committed
Refactor intake subsystem and override logic.
1 parent 2acddf6 commit e4fb63b

File tree

6 files changed

+51
-154
lines changed

6 files changed

+51
-154
lines changed

‎StateMachine.mmd‎

Lines changed: 23 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ stateDiagram-v2
2323
state PREP_ALGAE_WITH_CORAL_GROUP {
2424
PREP_ALGAE_NET_WITH_CORAL
2525
PREP_ALGAE_PROCESSOR_WITH_CORAL
26-
PREP_ALGAE_ZERO_WITH_CORAL
26+
PREP_CORAL_ZERO_WITH_ALGAE
2727
}
2828

2929
state INTAKE_CORAL_GROUP {
@@ -59,49 +59,49 @@ stateDiagram-v2
5959
CLIMBING
6060

6161
%% [*] - REPRESENTS NONE (starting and end)
62-
[*] --> INTAKE_CORAL_GROUP
63-
[*] --> INTAKE_ALGAE_GROUP
64-
[*] --> PREP_CLIMB
62+
[*] --> INTAKE_CORAL_GROUP: BTN = Left Trigger/Right Bumper
63+
[*] --> INTAKE_ALGAE_GROUP: BTN = Left Bumper/West/East
64+
[*] --> PREP_CLIMB: BTN = Driver BTN Start
6565
%% INTAKING
66-
INTAKE_ALGAE_GROUP --> [*]
67-
INTAKE_CORAL_GROUP --> [*]
66+
INTAKE_ALGAE_GROUP --> [*]: Has not collect algae
67+
INTAKE_CORAL_GROUP --> [*]: Has not collect coral
6868
INTAKE_CORAL_GROUP --> HAS_CORAL: Trigger = Has Coral
6969
INTAKE_ALGAE_GROUP --> HAS_ALGAE: Trigger = Has Algae
7070
INTAKE_CORAL_WITH_ALGAE_GROUP --> HAS_CORAL_AND_ALGAE: Trigger = Has Coral & Has Algae
7171
INTAKE_ALGAE_WITH_CORAL_GROUP --> HAS_CORAL_AND_ALGAE: Trigger = Has Coral & Has Algae
7272
%% HAS SCORING ELEMENT
7373
HAS_CORAL --> PREP_CORAL_GROUP: BTN = A/B/X/Y/Right Stick
7474
HAS_CORAL --> INTAKE_ALGAE_WITH_CORAL_GROUP: BTN = Left Trigger/Right Bumper
75-
HAS_CORAL --> PREP_CLIMB: BTN = Driver BTN = Start
76-
HAS_CORAL --> EJECTING
75+
HAS_CORAL --> PREP_CLIMB: BTN = Driver BTN Start
76+
HAS_CORAL --> EJECTING: BTN = Left Stick
7777
HAS_ALGAE --> INTAKE_CORAL_WITH_ALGAE_GROUP: BTN = Left Bumper/West/East
7878
HAS_ALGAE --> PREP_ALGAE_GROUP: BTN = North/South/Left Stick
79-
HAS_ALGAE --> PREP_CLIMB
80-
HAS_ALGAE --> EJECTING
79+
HAS_ALGAE --> PREP_CLIMB: BTN = Driver BTN Start
80+
HAS_ALGAE --> EJECTING: BTN = Left Stick
8181
HAS_CORAL_AND_ALGAE --> PREP_CORAL_WITH_ALGAE_GROUP: BTN = A/B/X/Y/Right Stick
82-
HAS_CORAL_AND_ALGAE --> PREP_ALGAE_WITH_CORAL_GROUP: BTN = North/South/Left Stick
83-
HAS_CORAL_AND_ALGAE --> PREP_CLIMB: BTN = Driver BTN = Start
84-
HAS_CORAL_AND_ALGAE --> EJECTING
82+
HAS_CORAL_AND_ALGAE --> PREP_ALGAE_WITH_CORAL_GROUP: BTN = North/South/Right Stick
83+
HAS_CORAL_AND_ALGAE --> PREP_CLIMB: BTN = Driver BTN Start
84+
HAS_CORAL_AND_ALGAE --> EJECTING: BTN = Left Stick
8585

8686
%% PREPPING
8787
PREP_CORAL_GROUP --> SCORING_CORAL: BTN = Right Trigger
8888
PREP_CORAL_WITH_ALGAE_GROUP --> SCORING_CORAL_WITH_ALGAE: BTN = Right Trigger
8989
PREP_ALGAE_GROUP --> SCORING_ALGAE: BTN = Right Trigger
9090
PREP_ALGAE_WITH_CORAL_GROUP --> SCORING_ALGAE_WITH_CORAL: BTN = Right Trigger
91-
PREP_ALGAE_WITH_CORAL_GROUP --> PREP_CORAL_WITH_ALGAE_GROUP
92-
PREP_CORAL_WITH_ALGAE_GROUP --> PREP_ALGAE_WITH_CORAL_GROUP
93-
PREP_ALGAE_GROUP --> EJECTING
94-
PREP_CORAL_GROUP --> EJECTING
95-
PREP_CORAL_WITH_ALGAE_GROUP --> EJECTING
96-
PREP_ALGAE_WITH_CORAL_GROUP --> EJECTING
91+
PREP_ALGAE_WITH_CORAL_GROUP --> PREP_CORAL_WITH_ALGAE_GROUP: BTN = A/B/X/Y/Right Stick
92+
PREP_CORAL_WITH_ALGAE_GROUP --> PREP_ALGAE_WITH_CORAL_GROUP: BTN = North/South/Left Stick
93+
PREP_ALGAE_GROUP --> EJECTING: BTN = Left Stick
94+
PREP_CORAL_GROUP --> EJECTING: BTN = Left Stick
95+
PREP_CORAL_WITH_ALGAE_GROUP --> EJECTING: BTN = Left Stick
96+
PREP_ALGAE_WITH_CORAL_GROUP --> EJECTING: BTN = Left Stick
9797
%% CLIMBING
9898
PREP_CLIMB --> CLIMBING: BTN = Driver BTN = Y
99-
PREP_CLIMB --> [*]
99+
PREP_CLIMB --> [*]: Automatically
100100
%% SCORING
101-
SCORING_CORAL --> [*]
102-
SCORING_ALGAE --> [*]
101+
SCORING_CORAL --> [*]: Automatically
102+
SCORING_ALGAE --> [*]: Automatically
103103
SCORING_CORAL_WITH_ALGAE --> HAS_ALGAE: Trigger = Has Algae
104104
SCORING_ALGAE_WITH_CORAL --> HAS_CORAL: Trigger = Has Coral
105-
EJECTING --> [*]
105+
EJECTING --> [*]: Automatically
106106

107107

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -246,9 +246,9 @@ public static boolean isRedAlliance() {
246246
public static class constElevator {
247247

248248
public static final Distance CORAL_L1_HEIGHT = Units.Inches.of(0);
249-
public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(0);
250-
public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(0);
251-
public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(0);
249+
public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(1);
250+
public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(2);
251+
public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(3);
252252

253253
}
254254

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 22 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ public class RobotContainer {
3636
private final Trigger hasCoralTrigger = new Trigger(() -> subIntake.hasCoral() && !subIntake.hasAlgae());
3737
private final Trigger hasAlgaeTrigger = new Trigger(() -> !subIntake.hasCoral() && subIntake.hasAlgae()
3838
&& subStateMachine.getRobotState() != RobotState.SCORING_CORAL_WITH_ALGAE
39-
&& subStateMachine.getRobotState() != RobotState.INTAKE_CORAL_WITH_ALGAE_GROUND);
39+
&& subStateMachine.getRobotState() != RobotState.INTAKE_CORAL_GROUND_WITH_ALGAE);
4040
private final Trigger hasBothTrigger = new Trigger(() -> subIntake.hasCoral() && subIntake.hasAlgae());
4141

4242
Command TRY_NONE = Commands.deferredProxy(
@@ -58,13 +58,13 @@ public class RobotContainer {
5858
Command TRY_PREP_CORAL_L4 = Commands.deferredProxy(
5959
() -> subStateMachine.tryState(RobotState.PREP_CORAL_L4));
6060
Command TRY_PREP_CORAL_WITH_ALGAE_L1 = Commands.deferredProxy(
61-
() -> subStateMachine.tryState(RobotState.PREP_CORAL_WITH_ALGAE_L1));
61+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_L1_WITH_ALGAE));
6262
Command TRY_PREP_CORAL_WITH_ALGAE_L2 = Commands.deferredProxy(
63-
() -> subStateMachine.tryState(RobotState.PREP_CORAL_WITH_ALGAE_L2));
63+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_L2_WITH_ALGAE));
6464
Command TRY_PREP_CORAL_WITH_ALGAE_L3 = Commands.deferredProxy(
65-
() -> subStateMachine.tryState(RobotState.PREP_CORAL_WITH_ALGAE_L3));
65+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_L3_WITH_ALGAE));
6666
Command TRY_PREP_CORAL_WITH_ALGAE_L4 = Commands.deferredProxy(
67-
() -> subStateMachine.tryState(RobotState.PREP_CORAL_WITH_ALGAE_L4));
67+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_L4_WITH_ALGAE));
6868
Command TRY_PREP_CORAL_ZERO_WITH_ALGAE = Commands.deferredProxy(
6969
() -> subStateMachine.tryState(RobotState.PREP_CORAL_ZERO_WITH_ALGAE));
7070
Command TRY_PREP_ALGAE_NET = Commands.deferredProxy(
@@ -77,8 +77,6 @@ public class RobotContainer {
7777
() -> subStateMachine.tryState(RobotState.PREP_ALGAE_NET_WITH_CORAL));
7878
Command TRY_PREP_ALGAE_PROCESSOR_WITH_CORAL = Commands.deferredProxy(
7979
() -> subStateMachine.tryState(RobotState.PREP_ALGAE_PROCESSOR_WITH_CORAL));
80-
Command TRY_PREP_ALGAE_ZERO_WITH_CORAL = Commands.deferredProxy(
81-
() -> subStateMachine.tryState(RobotState.PREP_ALGAE_ZERO_WITH_CORAL));
8280
Command TRY_HAS_CORAL = Commands.deferredProxy(
8381
() -> subStateMachine.tryState(RobotState.HAS_CORAL));
8482
Command TRY_HAS_ALGAE = Commands.deferredProxy(
@@ -109,17 +107,14 @@ public class RobotContainer {
109107
() -> subStateMachine.tryState(RobotState.CLEAN_LOW_WITH_CORAL));
110108
Command TRY_INTAKE_CORAL_GROUND = Commands.deferredProxy(
111109
() -> subStateMachine.tryState(RobotState.INTAKE_CORAL_GROUND));
112-
Command TRY_INTAKE_CORAL_WITH_ALGAE_GROUND = Commands.deferredProxy(
113-
() -> subStateMachine.tryState(RobotState.INTAKE_CORAL_WITH_ALGAE_GROUND));
114-
Command TRY_INTAKE_ALGAE_WITH_CORAL_GROUND = Commands.deferredProxy(
115-
() -> subStateMachine.tryState(RobotState.INTAKE_ALGAE_WITH_CORAL_GROUND));
116-
Command HAS_CORAL_OVERRIDE = Commands.deferredProxy(
117-
() -> subStateMachine.tryCoralOverride());
118-
Command HAS_ALGAE_OVERRIDE = Commands.runOnce(() -> subIntake.algaeToggle());
119-
Command TRY_NONE_FROM_SCORING = Commands.deferredProxy(
120-
() -> subStateMachine.tryState(RobotState.NONE)
121-
.unless(() -> (subStateMachine.getRobotState() == RobotState.SCORING_CORAL
122-
|| subStateMachine.getRobotState() == RobotState.SCORING_CORAL_WITH_ALGAE)));
110+
Command TRY_INTAKE_CORAL_GROUND_WITH_ALGAE = Commands.deferredProxy(
111+
() -> subStateMachine.tryState(RobotState.INTAKE_CORAL_GROUND_WITH_ALGAE));
112+
Command TRY_INTAKE_ALGAE_GROUND_WITH_CORAL = Commands.deferredProxy(
113+
() -> subStateMachine.tryState(RobotState.INTAKE_ALGAE_GROUND_WITH_CORAL));
114+
Command HAS_CORAL_OVERRIDE = Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.HAS_CORAL)
115+
.alongWith(subStateMachine.tryState(RobotState.HAS_CORAL_AND_ALGAE)));
116+
Command HAS_ALGAE_OVERRIDE = Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.HAS_ALGAE)
117+
.alongWith(subStateMachine.tryState(RobotState.HAS_CORAL_AND_ALGAE)));
123118

124119
public RobotContainer() {
125120
conDriver.setLeftDeadband(constControllers.DRIVER_LEFT_STICK_DEADBAND);
@@ -164,13 +159,13 @@ private void configOperatorBindings() {
164159
// Add operator bindings here if needed
165160
conOperator.btn_LeftTrigger
166161
.whileTrue(TRY_INTAKE_CORAL_GROUND)
167-
.whileTrue(TRY_INTAKE_CORAL_WITH_ALGAE_GROUND)
162+
.whileTrue(TRY_INTAKE_CORAL_GROUND_WITH_ALGAE)
168163
.onFalse(TRY_NONE)
169164
.onFalse(TRY_HAS_ALGAE);
170165

171166
conOperator.btn_LeftBumper
172167
.whileTrue(TRY_INTAKE_ALGAE_GROUND)
173-
.whileTrue(TRY_INTAKE_ALGAE_WITH_CORAL_GROUND)
168+
.whileTrue(TRY_INTAKE_ALGAE_GROUND_WITH_CORAL)
174169
.onFalse(TRY_NONE)
175170
.onFalse(TRY_HAS_CORAL);
176171

@@ -179,8 +174,9 @@ private void configOperatorBindings() {
179174
.whileTrue(TRY_SCORING_ALGAE)
180175
.whileTrue(TRY_SCORING_ALGAE_WITH_CORAL)
181176
.whileTrue(TRY_SCORING_CORAL_WITH_ALGAE)
182-
.onFalse(TRY_NONE_FROM_SCORING)
183-
.onFalse(TRY_HAS_CORAL);
177+
.onFalse(TRY_NONE)
178+
.onFalse(TRY_HAS_CORAL)
179+
.onFalse(TRY_HAS_ALGAE);
184180

185181
conOperator.btn_RightBumper
186182
.whileTrue(TRY_INTAKE_CORAL_STATION)
@@ -204,13 +200,13 @@ private void configOperatorBindings() {
204200
.onTrue(TRY_PREP_CORAL_WITH_ALGAE_L4);
205201

206202
conOperator.btn_LeftStick
207-
.onTrue(TRY_PREP_CORAL_ZERO)
208-
.onTrue(TRY_PREP_CORAL_ZERO_WITH_ALGAE);
209-
210-
conOperator.btn_RightStick
211203
.whileTrue(TRY_EJECTING)
212204
.onFalse(TRY_NONE);
213205

206+
conOperator.btn_RightStick
207+
.onTrue(TRY_PREP_CORAL_ZERO)
208+
.onTrue(TRY_PREP_CORAL_ZERO_WITH_ALGAE);
209+
214210
conOperator.btn_North
215211
.onTrue(TRY_PREP_ALGAE_NET)
216212
.onTrue(TRY_PREP_ALGAE_NET_WITH_CORAL);

‎src/main/java/frc/robot/commands/States/prep_algae/PrepAlgaeZeroWithCoral.java‎

Lines changed: 0 additions & 42 deletions
This file was deleted.

‎src/main/java/frc/robot/subsystems/Intake.java‎

Lines changed: 1 addition & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ public class Intake extends SubsystemBase {
1818
TalonFX coralRightMotor;
1919
TalonFX algaeIntakeMotor;
2020
public boolean hasCoral = false;
21-
public boolean hasAlgaeOverride = false;
21+
public boolean hasAlgae = false;
2222

2323
/** Creates a new Intake. */
2424
public Intake() {
@@ -35,14 +35,6 @@ public boolean hasCoral() {
3535
return hasCoral;
3636
}
3737

38-
public void setHasCoral(boolean hasCoral) {
39-
this.hasCoral = hasCoral;
40-
}
41-
42-
public void coralToggle() {
43-
this.hasCoral = !hasCoral;
44-
}
45-
4638
public boolean hasAlgae() {
4739
Current intakeCurrent = algaeIntakeMotor.getStatorCurrent().getValue();
4840

@@ -52,10 +44,6 @@ public boolean hasAlgae() {
5244
Current intakeHasGamePieceCurrent = constIntake.ALGAE_INTAKE_HAS_GP_CURRENT;
5345
AngularVelocity intakeHasGamePieceVelocity = constIntake.ALGAE_INTAKE_HAS_GP_VELOCITY;
5446

55-
if (hasAlgaeOverride) {
56-
return hasAlgaeOverride;
57-
}
58-
5947
if ((intakeCurrent.gte(intakeHasGamePieceCurrent))
6048
&& (intakeVelocity.lte(intakeHasGamePieceVelocity))
6149
&& (intakeAcceleration < 0)) {
@@ -65,14 +53,6 @@ public boolean hasAlgae() {
6553
}
6654
}
6755

68-
public void setHasAlgaeOverride(boolean passedHasGamePiece) {
69-
hasAlgaeOverride = passedHasGamePiece;
70-
}
71-
72-
public void algaeToggle() {
73-
this.hasAlgaeOverride = !hasAlgaeOverride;
74-
}
75-
7656
@Override
7757
public void periodic() {
7858
// This method will be called once per scheduler run

0 commit comments

Comments
 (0)