Skip to content

Commit 6c593e9

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #31 from strykeforce/robot-state
Adding Algae Mech to Robot State
2 parents 319bc73 + 80d290f commit 6c593e9

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

41 files changed

+1714
-402
lines changed

‎.vscode/settings.json‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,5 +57,5 @@
5757
"edu.wpi.first.math.**.proto.*",
5858
"edu.wpi.first.math.**.struct.*",
5959
],
60-
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
60+
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable"
6161
}

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,7 @@ dependencies {
8181

8282
implementation 'ch.qos.logback:logback-classic:1.3.5' //logging
8383
implementation("com.opencsv:opencsv:5.6")
84-
implementation 'org.strykeforce:wallEYE:25.0.0'
84+
implementation 'org.strykeforce:wallEYE:25.0.1'
8585
implementation 'net.jafama:jafama:2.3.2' //fastMath
8686
}
8787

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

Lines changed: 162 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -9,11 +9,17 @@
99
import edu.wpi.first.units.measure.Angle;
1010
import edu.wpi.first.wpilibj.Joystick;
1111
import edu.wpi.first.wpilibj.XboxController;
12+
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
1213
import edu.wpi.first.wpilibj2.command.Command;
1314
import edu.wpi.first.wpilibj2.command.Commands;
1415
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
1516
import edu.wpi.first.wpilibj2.command.button.Trigger;
17+
import frc.robot.commands.algae.IntakeAlgaeCommand;
1618
import frc.robot.commands.algae.OpenLoopAlgaeCommand;
19+
import frc.robot.commands.algae.ProcessorAlgaeCommand;
20+
import frc.robot.commands.algae.ToggleHasAlgaeCommand;
21+
import frc.robot.commands.biscuit.HoldBiscuitCommand;
22+
import frc.robot.commands.biscuit.JogBiscuitCommand;
1723
import frc.robot.commands.coral.EnableEjectBeamCommand;
1824
import frc.robot.commands.coral.OpenLoopCoralCommand;
1925
import frc.robot.commands.drive.DriveTeleopCommand;
@@ -22,9 +28,19 @@
2228
import frc.robot.commands.elevator.JogElevatorCommand;
2329
import frc.robot.commands.elevator.SetElevatorPositionCommand;
2430
import frc.robot.commands.elevator.ZeroElevatorCommand;
25-
import frc.robot.commands.robotState.ScoreReefManualCommand;
31+
import frc.robot.commands.robotState.FloorAlgaeCommand;
32+
import frc.robot.commands.robotState.HPAlgaeCommand;
33+
import frc.robot.commands.robotState.InterruptAutoCommand;
34+
import frc.robot.commands.robotState.ReefCycleCommand;
35+
import frc.robot.commands.robotState.ScoreAlgaeCommand;
36+
import frc.robot.commands.robotState.SetScoreSideRightCommand;
2637
import frc.robot.commands.robotState.SetScoringLevelCommand;
2738
import frc.robot.commands.robotState.StowCommand;
39+
import frc.robot.commands.robotState.ToggleAlgaeHeightCommand;
40+
import frc.robot.commands.robotState.ToggleAutoCommand;
41+
import frc.robot.commands.robotState.ToggleGetAlgaeCommand;
42+
import frc.robot.commands.robotState.setScoreSideLeftCommand;
43+
import frc.robot.constants.BiscuitConstants;
2844
import frc.robot.constants.ElevatorConstants;
2945
import frc.robot.constants.RobotConstants;
3046
import frc.robot.controllers.FlyskyJoystick;
@@ -119,7 +135,7 @@ public RobotContainer() {
119135
ledIO = new LEDIO();
120136
ledSubsystem = new LEDSubsystem();
121137

122-
visionSubsystem = new VisionSubsystem();
138+
visionSubsystem = new VisionSubsystem(driveSubsystem);
123139

124140
tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem);
125141

@@ -142,6 +158,8 @@ public RobotContainer() {
142158
configureTelemetry();
143159
configureDriverBindings();
144160
configureOperatorBindings();
161+
// configureTestOperatorBindings();
162+
configurePitDashboard();
145163
}
146164

147165
private void configureTelemetry() {
@@ -150,6 +168,7 @@ private void configureTelemetry() {
150168
algaeSubsystem.registerWith(telemetryService);
151169
elevatorSubsystem.registerWith(telemetryService);
152170
funnelSubsystem.registerWith(telemetryService);
171+
biscuitSubsystem.registerWith(telemetryService);
153172
telemetryService.start();
154173
}
155174

@@ -158,13 +177,79 @@ private void configureDriverBindings() {
158177
new DriveTeleopCommand(
159178
() -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem));
160179

161-
// Reset Gyro Command
180+
// Reset Gyro Command, stow, interrupt Auton, and zero elev
162181
new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem));
182+
new JoystickButton(driveJoystick, Button.SWD.id)
183+
.onTrue(
184+
new StowCommand(
185+
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem))
186+
.onFalse(
187+
new StowCommand(
188+
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
189+
new JoystickButton(driveJoystick, Button.SWA.id)
190+
.onTrue(new InterruptAutoCommand(robotStateSubsystem))
191+
.onFalse(new InterruptAutoCommand(robotStateSubsystem));
192+
new JoystickButton(driveJoystick, Button.SWB_UP.id)
193+
.onTrue(new ZeroElevatorCommand(elevatorSubsystem))
194+
.onFalse(new ZeroElevatorCommand(elevatorSubsystem));
195+
new JoystickButton(driveJoystick, Button.SWB_DWN.id)
196+
.onTrue(new ZeroElevatorCommand(elevatorSubsystem))
197+
.onFalse(new ZeroElevatorCommand(elevatorSubsystem));
198+
199+
// other stuff
163200
new JoystickButton(driveJoystick, Button.M_SWH.id)
164-
.onTrue(new ScoreReefManualCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem));
201+
.onTrue(
202+
new ReefCycleCommand(
203+
robotStateSubsystem,
204+
elevatorSubsystem,
205+
coralSubsystem,
206+
biscuitSubsystem,
207+
algaeSubsystem));
208+
new JoystickButton(driveJoystick, Button.M_SWE.id)
209+
.onTrue(
210+
new ScoreAlgaeCommand(
211+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
212+
new JoystickButton(driveJoystick, Button.SWF_UP.id)
213+
.onTrue(
214+
new FloorAlgaeCommand(
215+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
216+
.onFalse(
217+
new FloorAlgaeCommand(
218+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
219+
new JoystickButton(driveJoystick, Button.SWF_DWN.id)
220+
.onTrue(
221+
new FloorAlgaeCommand(
222+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
223+
.onFalse(
224+
new FloorAlgaeCommand(
225+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
165226
}
166227

167228
private void configureOperatorBindings() {
229+
// Move biscuit
230+
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
231+
.onTrue(
232+
new JogBiscuitCommand(
233+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
234+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
235+
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
236+
.onTrue(
237+
new JogBiscuitCommand(
238+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
239+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
240+
241+
// Move elevator
242+
new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
243+
.onTrue(
244+
new JogElevatorCommand(
245+
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
246+
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
247+
new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
248+
.onTrue(
249+
new JogElevatorCommand(
250+
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
251+
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
252+
168253
// Set Levels
169254
new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband)
170255
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1));
@@ -181,12 +266,56 @@ private void configureOperatorBindings() {
181266
new StowCommand(
182267
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
183268

184-
// zero elevator, slated for removal
269+
// Algae
270+
new JoystickButton(xboxController, XboxController.Button.kY.value)
271+
.onTrue(new ToggleAlgaeHeightCommand(robotStateSubsystem));
185272
new JoystickButton(xboxController, XboxController.Button.kX.value)
186-
.onTrue(new ZeroElevatorCommand(elevatorSubsystem));
273+
.onTrue(new ToggleGetAlgaeCommand(robotStateSubsystem));
274+
new JoystickButton(xboxController, XboxController.Button.kB.value)
275+
.onTrue(
276+
new HPAlgaeCommand(
277+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
278+
279+
// Scoring
280+
new JoystickButton(xboxController, XboxController.Button.kA.value)
281+
.onTrue(new ToggleAutoCommand(robotStateSubsystem));
282+
283+
new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
284+
.onTrue(new SetScoreSideRightCommand(robotStateSubsystem));
285+
new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
286+
.onTrue(new setScoreSideLeftCommand(robotStateSubsystem));
187287
}
188288

189289
private void configureTestOperatorBindings() {
290+
new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
291+
.onTrue(new IntakeAlgaeCommand(algaeSubsystem));
292+
new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
293+
.onTrue(new ProcessorAlgaeCommand(algaeSubsystem));
294+
295+
// Move biscuit
296+
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
297+
.onTrue(
298+
new JogBiscuitCommand(
299+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
300+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
301+
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
302+
.onTrue(
303+
new JogBiscuitCommand(
304+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
305+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
306+
307+
// Move elevator
308+
new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
309+
.onTrue(
310+
new JogElevatorCommand(
311+
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
312+
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
313+
new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
314+
.onTrue(
315+
new JogElevatorCommand(
316+
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
317+
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
318+
190319
// Stop Coral
191320
new JoystickButton(xboxController, XboxController.Button.kB.value)
192321
.onTrue(new OpenLoopCoralCommand(coralSubsystem, 0));
@@ -201,13 +330,25 @@ private void configureTestOperatorBindings() {
201330
.onTrue(new OpenLoopCoralCommand(coralSubsystem, 1))
202331
.onTrue(new EnableEjectBeamCommand(false, coralSubsystem));
203332

204-
// Move Elevator
333+
// Move biscuit
205334
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
335+
.onTrue(
336+
new JogBiscuitCommand(
337+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
338+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
339+
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
340+
.onTrue(
341+
new JogBiscuitCommand(
342+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
343+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
344+
345+
// Move elevator
346+
new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
206347
.onTrue(
207348
new JogElevatorCommand(
208349
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
209350
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
210-
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
351+
new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
211352
.onTrue(
212353
new JogElevatorCommand(
213354
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
@@ -243,6 +384,19 @@ private void configureTestOperatorBindings() {
243384
new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL4CoralSetpoint));
244385
}
245386

387+
public void configurePitDashboard() {
388+
389+
Shuffleboard.getTab("Pit")
390+
.add("Toggle Has Algae", new ToggleHasAlgaeCommand(algaeSubsystem))
391+
.withPosition(3, 2)
392+
.withSize(1, 1);
393+
394+
Shuffleboard.getTab("Pit")
395+
.add("Zero Elevator", new ZeroElevatorCommand(elevatorSubsystem))
396+
.withPosition(2, 1)
397+
.withSize(1, 1);
398+
}
399+
246400
public Command getAutonomousCommand() {
247401
return Commands.print("No autonomous command configured");
248402
}

‎src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
package frc.robot.commands.algae;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.algae.AlgaeSubsystem;
5+
6+
public class IntakeAlgaeCommand extends InstantCommand {
7+
private AlgaeSubsystem algaeSubsystem;
8+
9+
public IntakeAlgaeCommand(AlgaeSubsystem algaeSubsystem) {
10+
this.algaeSubsystem = algaeSubsystem;
11+
}
12+
13+
@Override
14+
public void initialize() {
15+
algaeSubsystem.intake();
16+
}
17+
}

‎src/main/java/frc/robot/commands/algae/ProcessorAlgaeCommand.java‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
package frc.robot.commands.algae;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.algae.AlgaeSubsystem;
5+
6+
public class ProcessorAlgaeCommand extends InstantCommand {
7+
private AlgaeSubsystem algaeSubsystem;
8+
9+
public ProcessorAlgaeCommand(AlgaeSubsystem algaeSubsystem) {
10+
this.algaeSubsystem = algaeSubsystem;
11+
}
12+
13+
@Override
14+
public void initialize() {
15+
algaeSubsystem.scoreProcessor();
16+
}
17+
}

‎src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
package frc.robot.commands.algae;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.algae.AlgaeSubsystem;
5+
import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeStates;
6+
7+
public class ToggleHasAlgaeCommand extends InstantCommand {
8+
private AlgaeSubsystem algaeSubsystem;
9+
10+
public ToggleHasAlgaeCommand(AlgaeSubsystem algaeSubsystem) {
11+
this.algaeSubsystem = algaeSubsystem;
12+
}
13+
14+
@Override
15+
public void initialize() {
16+
algaeSubsystem.setState(
17+
algaeSubsystem.getState() == AlgaeStates.EMPTY ? AlgaeStates.HAS_ALGAE : AlgaeStates.EMPTY);
18+
}
19+
}

‎src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
package frc.robot.commands.biscuit;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
5+
6+
public class HoldBiscuitCommand extends InstantCommand {
7+
private BiscuitSubsystem biscuitSubsystem;
8+
9+
public HoldBiscuitCommand(BiscuitSubsystem biscuitSubsystem) {
10+
this.biscuitSubsystem = biscuitSubsystem;
11+
addRequirements(biscuitSubsystem);
12+
}
13+
14+
@Override
15+
public void initialize() {
16+
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition());
17+
}
18+
}

‎src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
package frc.robot.commands.biscuit;
2+
3+
import edu.wpi.first.units.measure.Angle;
4+
import edu.wpi.first.wpilibj2.command.Command;
5+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
6+
7+
public class JogBiscuitCommand extends Command {
8+
9+
private BiscuitSubsystem biscuitSubsystem;
10+
private Angle positionChange;
11+
12+
public JogBiscuitCommand(BiscuitSubsystem biscuitSubsystem, Angle positionChange) {
13+
this.biscuitSubsystem = biscuitSubsystem;
14+
this.positionChange = positionChange;
15+
addRequirements(biscuitSubsystem);
16+
}
17+
18+
@Override
19+
public void initialize() {
20+
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange));
21+
}
22+
23+
@Override
24+
public void execute() {
25+
biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange));
26+
}
27+
28+
@Override
29+
public boolean isFinished() {
30+
return false;
31+
}
32+
}

‎src/main/java/frc/robot/commands/robotState/ClimbCommand.java‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
5+
6+
public class ClimbCommand extends InstantCommand {
7+
RobotStateSubsystem robotState;
8+
9+
public ClimbCommand(RobotStateSubsystem robotState) {
10+
this.robotState = robotState;
11+
}
12+
13+
@Override
14+
public void initialize() {
15+
robotState.toClimb();
16+
}
17+
}

0 commit comments

Comments
 (0)