Skip to content

Commit da0fd55

Browse files
committed
Add main robot state machine
1 parent 307d689 commit da0fd55

File tree

1 file changed

+293
-0
lines changed

1 file changed

+293
-0
lines changed
Lines changed: 293 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,293 @@
1+
package frc.robot;
2+
3+
import java.util.function.BooleanSupplier;
4+
import java.util.function.DoubleSupplier;
5+
6+
import org.lasarobotics.fsm.StateMachine;
7+
import org.lasarobotics.fsm.SystemState;
8+
9+
import frc.robot.subsystems.drivetrain.DriveSubsystem;
10+
import frc.robot.subsystems.lift.LiftSubsystem;
11+
12+
public class RobotState extends StateMachine implements AutoCloseable{
13+
public static record Hardware() {}
14+
15+
public enum State implements SystemState {
16+
REST {
17+
@Override
18+
public void initialize() {
19+
// reset the whole robot
20+
END_EFFECTOR_SUBSYSTEM.stop();
21+
DRIVE_SUBSYSTEM.cancelAutoAlign();
22+
LIFT_SUBSYSTEM.goToState(STOW);
23+
INTAKE_SUBSYSTEM.stop();
24+
}
25+
26+
@Override
27+
public SystemState nextState() {
28+
if (intake_button.getAsBoolean() && END_EFFECTOR_SUBSYSTEM.isEmpty() && LIFT_SUBSYSTEM.isAtState(STOW)) {
29+
return INTAKE;
30+
}
31+
32+
if (DRIVE_SUBSYSTEM.nearSource() && END_EFFECTOR_SUBSYSTEM.isEmpty() && LIFT_SUBSYSTEM.isAtState(STOW)) {
33+
return AUTO_INTAKE;
34+
}
35+
36+
if (regurgitate_button.getAsBoolean() && LIFT_SUBSYSTEM.isAtState(STOW)) {
37+
return REGURGITATE;
38+
}
39+
40+
if (l1_button.getAsBoolean() && END_EFFECTOR_SUBSYSTEM.hasCoral()) {
41+
return L1;
42+
}
43+
if (l2_button.getAsBoolean() && END_EFFECTOR_SUBSYSTEM.hasCoral()) {
44+
return L2;
45+
}
46+
if (l3_button.getAsBoolean() && END_EFFECTOR_SUBSYSTEM.hasCoral()) {
47+
return L3;
48+
}
49+
if (l4_button.getAsBoolean() && END_EFFECTOR_SUBSYSTEM.hasCoral()) {
50+
return L4;
51+
}
52+
}
53+
},
54+
INTAKE {
55+
@Override
56+
public void initialize() {
57+
INTAKE_SUBSYSTEM.intake();
58+
END_EFFECTOR_SUBSYSTEM.intake();
59+
}
60+
61+
@Override
62+
public SystemState nextState() {
63+
if (END_EFFECTOR_SUBSYSTEM.hasCoral()) return REST;
64+
if (!intake_button.getAsBoolean() && END_EFFECTOR_SUBSYSTEM.isEmpty()) return REST;
65+
}
66+
67+
@Override
68+
public void end(boolean interrupted) {
69+
INTAKE_SUBSYSTEM.stop();
70+
END_EFFECTOR_SUBSYSTEM.stop();
71+
}
72+
},
73+
AUTO_INTAKE {
74+
@Override
75+
public void initialize() {
76+
INTAKE_SUBSYSTEM.intake();
77+
END_EFFECTOR_SUBSYSTEM.intake();
78+
}
79+
80+
@Override
81+
public SystemState nextState() {
82+
if (END_EFFECTOR_SUBSYSTEM.hasCoral()) return REST;
83+
}
84+
85+
@Override
86+
public void end(boolean interrupted) {
87+
INTAKE_SUBSYSTEM.stop();
88+
END_EFFECTOR_SUBSYSTEM.stop();
89+
}
90+
},
91+
REGURGITATE {
92+
@Override
93+
public void initialize() {
94+
END_EFFECTOR_SUBSYSTEM.regurgitate();
95+
INTAKE_SUBSYSTEM.regurgitate();
96+
}
97+
98+
@Override
99+
public SystemState nextState() {
100+
if (!regurgitate_button.getAsBoolean() && (INTAKE_SUBSYSTEM.hasCoral() || INTAKE_SUBSYSTEM.isEmpty())) return REST;
101+
// TODO algae
102+
}
103+
104+
@Override
105+
public void end(boolean interrupted) {
106+
INTAKE_SUBSYSTEM.stop();
107+
END_EFFECTOR_SUBSYSTEM.stop();
108+
}
109+
},
110+
111+
L1 {
112+
@Override
113+
public void initialize() {
114+
LIFT_SUBSYSTEM.goToState(L1);
115+
DRIVE_SUBSYSTEM.requestAutoAlign();
116+
}
117+
118+
@Override
119+
public SystemState nextState() {
120+
if (LIFT_SUBSYSTEM.isAtState(L1) && DRIVE_SUBSYSTEM.isAligned()) return SCORE;
121+
if (score_button.getAsBoolean()) return SCORE;
122+
123+
if (l1_button.getAsBoolean()) return L1;
124+
if (l2_button.getAsBoolean()) return L2;
125+
if (l3_button.getAsBoolean()) return L3;
126+
if (l4_button.getAsBoolean()) return L4;
127+
}
128+
},
129+
L2 {
130+
@Override
131+
public void initialize() {
132+
LIFT_SUBSYSTEM.goToState(L2);
133+
DRIVE_SUBSYSTEM.requestAutoAlign();
134+
}
135+
136+
@Override
137+
public SystemState nextState() {
138+
if (LIFT_SUBSYSTEM.isAtState(L2) && DRIVE_SUBSYSTEM.isAligned()) return SCORE;
139+
if (score_button.getAsBoolean()) return SCORE;
140+
141+
if (l1_button.getAsBoolean()) return L1;
142+
if (l2_button.getAsBoolean()) return L2;
143+
if (l3_button.getAsBoolean()) return L3;
144+
if (l4_button.getAsBoolean()) return L4;
145+
}
146+
},
147+
L3 {
148+
@Override
149+
public void initialize() {
150+
LIFT_SUBSYSTEM.goToState(L3);
151+
DRIVE_SUBSYSTEM.requestAutoAlign();
152+
}
153+
154+
@Override
155+
public SystemState nextState() {
156+
if (LIFT_SUBSYSTEM.isAtState(L3) && DRIVE_SUBSYSTEM.isAligned()) return SCORE;
157+
if (score_button.getAsBoolean()) return SCORE;
158+
159+
if (l1_button.getAsBoolean()) return L1;
160+
if (l2_button.getAsBoolean()) return L2;
161+
if (l3_button.getAsBoolean()) return L3;
162+
if (l4_button.getAsBoolean()) return L4;
163+
}
164+
},
165+
L4 {
166+
@Override
167+
public void initialize() {
168+
LIFT_SUBSYSTEM.goToState(L4);
169+
DRIVE_SUBSYSTEM.requestAutoAlign();
170+
}
171+
172+
@Override
173+
public SystemState nextState() {
174+
if (LIFT_SUBSYSTEM.isAtState(L4) && DRIVE_SUBSYSTEM.isAligned()) return SCORE_L4;
175+
if (score_button.getAsBoolean()) return SCORE;
176+
177+
if (l1_button.getAsBoolean()) return L1;
178+
if (l2_button.getAsBoolean()) return L2;
179+
if (l3_button.getAsBoolean()) return L3;
180+
if (l4_button.getAsBoolean()) return L4;
181+
}
182+
},
183+
184+
SCORE {
185+
@Override
186+
public void initialize() {
187+
END_EFFECTOR_SUBSYSTEM.outtake();
188+
}
189+
190+
@Override
191+
public SystemState nextState() {
192+
if (END_EFFECTOR_SUBSYSTEM.isEmpty()) return REST;
193+
}
194+
195+
@Override
196+
public void end(boolean interrupted) {
197+
LIFT_SUBSYSTEM.goToState(STOW);
198+
END_EFFECTOR_SUBSYSTEM.stop();
199+
DRIVE_SUBSYSTEM.cancelAutoAlign();
200+
}
201+
},
202+
SCORE_L4 {
203+
@Override
204+
public void initialize() {
205+
END_EFFECTOR_SUBSYSTEM.outtake_reverse();
206+
}
207+
208+
@Override
209+
public SystemState nextState() {
210+
if (END_EFFECTOR_SUBSYSTEM.isEmpty()) return REST;
211+
}
212+
213+
@Override
214+
public void end(boolean interrupted) {
215+
LIFT_SUBSYSTEM.goToState(STOW);
216+
END_EFFECTOR_SUBSYSTEM.stop();
217+
DRIVE_SUBSYSTEM.cancelAutoAlign();
218+
}
219+
}
220+
}
221+
222+
// Subsystems
223+
private static DriveSubsystem DRIVE_SUBSYSTEM;
224+
private static IntakeSubsystem INTAKE_SUBSYSTEM;
225+
private static LiftSubsystem LIFT_SUBSYSTEM;
226+
private static EndEffectorSubsystem END_EFFECTOR_SUBSYSTEM;
227+
private static ClimberSubsystem CLIMB_SUBSYSTEM;
228+
229+
// User input
230+
private static DoubleSupplier drive_x_request;
231+
private static DoubleSupplier drive_y_request;
232+
private static DoubleSupplier drive_rotate_request;
233+
234+
private static BooleanSupplier intake_button;
235+
private static BooleanSupplier regurgitate_button;
236+
237+
private static BooleanSupplier l1_button;
238+
private static BooleanSupplier l2_button;
239+
private static BooleanSupplier l3_button;
240+
private static BooleanSupplier l4_button;
241+
242+
private static BooleanSupplier score_button; // force robot to score, regardless of alignment
243+
244+
public RobotState(
245+
Hardware hardware,
246+
DriveSubsystem drive_subsystem,
247+
IntakeSubsystem intake_subsystem,
248+
LiftSubsystem lift_subsystem,
249+
EndEffectorSubsystem end_effector_subsystem,
250+
ClimberSubsystem climb_subsystem
251+
) {
252+
super(State.REST);
253+
254+
DRIVE_SUBSYSTEM = drive_subsystem;
255+
INTAKE_SUBSYSTEM = intake_subsystem;
256+
LIFT_SUBSYSTEM = lift_subsystem;
257+
END_EFFECTOR_SUBSYSTEM = end_effector_subsystem;
258+
CLIMB_SUBSYSTEM = climb_subsystem;
259+
}
260+
261+
public void bindControls(
262+
DoubleSupplier drive_x_reqeust,
263+
DoubleSupplier drive_y_request,
264+
DoubleSupplier drive_rotate_request,
265+
BooleanSupplier intake_button,
266+
BooleanSupplier regurgitate_button,
267+
BooleanSupplier l1_button,
268+
BooleanSupplier l2_button,
269+
BooleanSupplier l3_button,
270+
BooleanSupplier l4_button,
271+
BooleanSupplier score_button
272+
) {
273+
RobotState.drive_x_request = drive_x_reqeust;
274+
RobotState.drive_y_request = drive_y_request;
275+
RobotState.drive_rotate_request = drive_rotate_request;
276+
277+
RobotState.intake_button = intake_button;
278+
RobotState.regurgitate_button = regurgitate_button;
279+
RobotState.l1_button = l1_button;
280+
RobotState.l2_button = l2_button;
281+
RobotState.l3_button = l3_button;
282+
RobotState.l4_button = l4_button;
283+
284+
RobotState.score_button = score_button;
285+
286+
DRIVE_SUBSYSTEM.bindControls(drive_x_reqeust, drive_y_request, drive_rotate_request);
287+
}
288+
289+
@Override
290+
public void close() throws Exception {
291+
292+
}
293+
}

0 commit comments

Comments
 (0)