Skip to content

Commit b8778d0

Browse files
ds12ads12a
committed
Merge branch 'robot-state' into tag-align
2 parents 31d3123 + 8a1200d commit b8778d0

36 files changed

+987
-270
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: 137 additions & 7 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;
@@ -23,11 +29,21 @@
2329
import frc.robot.commands.elevator.SetElevatorPositionCommand;
2430
import frc.robot.commands.elevator.ZeroElevatorCommand;
2531
import frc.robot.commands.robotState.AutoScoreReefCommand;
26-
import frc.robot.commands.robotState.ScoreReefManualCommand;
32+
import frc.robot.commands.robotState.FloorAlgaeCommand;
33+
import frc.robot.commands.robotState.HPAlgaeCommand;
34+
import frc.robot.commands.robotState.InterruptAutoCommand;
35+
import frc.robot.commands.robotState.ReefCycleCommand;
36+
import frc.robot.commands.robotState.ScoreAlgaeCommand;
37+
import frc.robot.commands.robotState.SetScoreSideRightCommand;
2738
import frc.robot.commands.robotState.SetScoreSideCommand;
2839
import frc.robot.commands.robotState.SetScoringLevelCommand;
2940
import frc.robot.commands.robotState.StowCommand;
3041
import frc.robot.commands.tagAlign.TagAlignCommand;
42+
import frc.robot.commands.robotState.ToggleAlgaeHeightCommand;
43+
import frc.robot.commands.robotState.ToggleAutoCommand;
44+
import frc.robot.commands.robotState.ToggleGetAlgaeCommand;
45+
import frc.robot.commands.robotState.setScoreSideLeftCommand;
46+
import frc.robot.constants.BiscuitConstants;
3147
import frc.robot.constants.ElevatorConstants;
3248
import frc.robot.constants.RobotConstants;
3349
import frc.robot.controllers.FlyskyJoystick;
@@ -146,6 +162,8 @@ public RobotContainer() {
146162
configureTelemetry();
147163
configureDriverBindings();
148164
configureOperatorBindings();
165+
// configureTestOperatorBindings();
166+
configurePitDashboard();
149167
}
150168

151169
private void configureTelemetry() {
@@ -154,6 +172,7 @@ private void configureTelemetry() {
154172
algaeSubsystem.registerWith(telemetryService);
155173
elevatorSubsystem.registerWith(telemetryService);
156174
funnelSubsystem.registerWith(telemetryService);
175+
biscuitSubsystem.registerWith(telemetryService);
157176
telemetryService.start();
158177
}
159178

@@ -168,10 +187,52 @@ private void configureDriverBindings() {
168187
new DriveTeleopCommand(
169188
() -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem));
170189

171-
// Reset Gyro Command
190+
// Reset Gyro Command, stow, interrupt Auton, and zero elev
172191
new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem));
192+
new JoystickButton(driveJoystick, Button.SWD.id)
193+
.onTrue(
194+
new StowCommand(
195+
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem))
196+
.onFalse(
197+
new StowCommand(
198+
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
199+
new JoystickButton(driveJoystick, Button.SWA.id)
200+
.onTrue(new InterruptAutoCommand(robotStateSubsystem))
201+
.onFalse(new InterruptAutoCommand(robotStateSubsystem));
202+
new JoystickButton(driveJoystick, Button.SWB_UP.id)
203+
.onTrue(new ZeroElevatorCommand(elevatorSubsystem))
204+
.onFalse(new ZeroElevatorCommand(elevatorSubsystem));
205+
new JoystickButton(driveJoystick, Button.SWB_DWN.id)
206+
.onTrue(new ZeroElevatorCommand(elevatorSubsystem))
207+
.onFalse(new ZeroElevatorCommand(elevatorSubsystem));
208+
209+
// other stuff
173210
new JoystickButton(driveJoystick, Button.M_SWH.id)
174-
.onTrue(new ScoreReefManualCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem));
211+
.onTrue(
212+
new ReefCycleCommand(
213+
robotStateSubsystem,
214+
elevatorSubsystem,
215+
coralSubsystem,
216+
biscuitSubsystem,
217+
algaeSubsystem));
218+
new JoystickButton(driveJoystick, Button.M_SWE.id)
219+
.onTrue(
220+
new ScoreAlgaeCommand(
221+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
222+
new JoystickButton(driveJoystick, Button.SWF_UP.id)
223+
.onTrue(
224+
new FloorAlgaeCommand(
225+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
226+
.onFalse(
227+
new FloorAlgaeCommand(
228+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
229+
new JoystickButton(driveJoystick, Button.SWF_DWN.id)
230+
.onTrue(
231+
new FloorAlgaeCommand(
232+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
233+
.onFalse(
234+
new FloorAlgaeCommand(
235+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
175236
}
176237

177238
private void configureOperatorBindings() {
@@ -201,12 +262,56 @@ private void configureOperatorBindings() {
201262
new StowCommand(
202263
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
203264

204-
// zero elevator, slated for removal
265+
// Algae
266+
new JoystickButton(xboxController, XboxController.Button.kY.value)
267+
.onTrue(new ToggleAlgaeHeightCommand(robotStateSubsystem));
205268
new JoystickButton(xboxController, XboxController.Button.kX.value)
206-
.onTrue(new ZeroElevatorCommand(elevatorSubsystem));
269+
.onTrue(new ToggleGetAlgaeCommand(robotStateSubsystem));
270+
new JoystickButton(xboxController, XboxController.Button.kB.value)
271+
.onTrue(
272+
new HPAlgaeCommand(
273+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
274+
275+
// Scoring
276+
new JoystickButton(xboxController, XboxController.Button.kA.value)
277+
.onTrue(new ToggleAutoCommand(robotStateSubsystem));
278+
279+
new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
280+
.onTrue(new SetScoreSideRightCommand(robotStateSubsystem));
281+
new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
282+
.onTrue(new setScoreSideLeftCommand(robotStateSubsystem));
207283
}
208284

209285
private void configureTestOperatorBindings() {
286+
new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
287+
.onTrue(new IntakeAlgaeCommand(algaeSubsystem));
288+
new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
289+
.onTrue(new ProcessorAlgaeCommand(algaeSubsystem));
290+
291+
// Move biscuit
292+
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
293+
.onTrue(
294+
new JogBiscuitCommand(
295+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
296+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
297+
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
298+
.onTrue(
299+
new JogBiscuitCommand(
300+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
301+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
302+
303+
// Move elevator
304+
new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
305+
.onTrue(
306+
new JogElevatorCommand(
307+
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
308+
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
309+
new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
310+
.onTrue(
311+
new JogElevatorCommand(
312+
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
313+
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
314+
210315
// Stop Coral
211316
new JoystickButton(xboxController, XboxController.Button.kB.value)
212317
.onTrue(new OpenLoopCoralCommand(coralSubsystem, 0));
@@ -221,13 +326,25 @@ private void configureTestOperatorBindings() {
221326
.onTrue(new OpenLoopCoralCommand(coralSubsystem, 1))
222327
.onTrue(new EnableEjectBeamCommand(false, coralSubsystem));
223328

224-
// Move Elevator
329+
// Move biscuit
225330
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
331+
.onTrue(
332+
new JogBiscuitCommand(
333+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
334+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
335+
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
336+
.onTrue(
337+
new JogBiscuitCommand(
338+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
339+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
340+
341+
// Move elevator
342+
new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
226343
.onTrue(
227344
new JogElevatorCommand(
228345
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
229346
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
230-
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
347+
new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
231348
.onTrue(
232349
new JogElevatorCommand(
233350
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
@@ -263,6 +380,19 @@ private void configureTestOperatorBindings() {
263380
new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL4CoralSetpoint));
264381
}
265382

383+
public void configurePitDashboard() {
384+
385+
Shuffleboard.getTab("Pit")
386+
.add("Toggle Has Algae", new ToggleHasAlgaeCommand(algaeSubsystem))
387+
.withPosition(3, 2)
388+
.withSize(1, 1);
389+
390+
Shuffleboard.getTab("Pit")
391+
.add("Zero Elevator", new ZeroElevatorCommand(elevatorSubsystem))
392+
.withPosition(2, 1)
393+
.withSize(1, 1);
394+
}
395+
266396
public Command getAutonomousCommand() {
267397
return Commands.print("No autonomous command configured");
268398
}

‎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+
}

‎src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.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 ClimbPrepCommand extends InstantCommand {
7+
RobotStateSubsystem robotState;
8+
9+
public ClimbPrepCommand(RobotStateSubsystem robotState) {
10+
this.robotState = robotState;
11+
}
12+
13+
@Override
14+
public void initialize() {
15+
robotState.toPrepClimb();
16+
}
17+
}

0 commit comments

Comments
 (0)