Skip to content

Commit 8b4a47d

Browse files
nlaverdurenlaverdure
authored
Variable coral outtaking (#146)
* create multiple outtaking commands * add operator button logic * use new command in autos * break intaking commands from parallel group
1 parent a2f67e1 commit 8b4a47d

12 files changed

+80
-49
lines changed

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

Lines changed: 17 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -334,13 +334,19 @@ private void configureDriverButtonBindings() {
334334

335335
// Outtake grippers
336336
var outtaking = driver.HIn();
337-
lifter.atProcessorHeight.and(outtaking)
338-
.whileTrue(algaeRoller.createOuttakeToProcessorCommand());
339-
lifter.atProcessorHeight.negate().and(outtaking)
340-
.whileTrue(algaeRoller.createOuttakeToBargeCommand());
341-
outtaking
342-
.whileTrue(coralRoller.createOuttakeCommand()
343-
.alongWith(new InstantCommand(() -> rememberOutputPose(swerve.getPose()))));
337+
outtaking.onTrue(new InstantCommand(() -> rememberOutputPose(swerve.getPose())));
338+
lifter.atProcessor.and(outtaking)
339+
.whileTrue(algaeRoller.outtakeToProcessor());
340+
lifter.atAlgaeElse.and(outtaking)
341+
.whileTrue(algaeRoller.outtakeToBarge());
342+
lifter.atCoralL1.and(outtaking)
343+
.whileTrue(coralRoller.outtakeToL1());
344+
lifter.atCoralL2.and(outtaking)
345+
.whileTrue(coralRoller.outtakeToL2());
346+
lifter.atCoralL3.and(outtaking)
347+
.whileTrue(coralRoller.outtakeToL3());
348+
lifter.atCoralElse.and(outtaking)
349+
.whileTrue(coralRoller.outtakeToL4());
344350
}
345351

346352
private void configureOperatorButtonBindings() {
@@ -394,8 +400,9 @@ public boolean getAsBoolean() {
394400

395401
// Intake coral and algae
396402
operator.rightBumper()
397-
.whileTrue(algaeRoller.createIntakeCommand()
398-
.alongWith(coralRoller.createIntakeCommand()));
403+
.whileTrue(algaeRoller.intake());
404+
operator.rightBumper()
405+
.whileTrue(coralRoller.intake());
399406

400407
// Force joystick operation of the elevator
401408
Trigger elevatorTriggerHigh = operator.axisGreaterThan(Axis.kLeftY.value, 0.9, loop).debounce(0.1);
@@ -443,7 +450,7 @@ private void configureEventBindings() {
443450
.onTrue(climber.lockRatchet().andThen(climber.resetEncoder()));
444451

445452
algaeRoller.hasAlgae.onTrue(elevator.holdAlgaeCG());
446-
coralRoller.hasCoral.onTrue(coralRoller.createStopCommand());
453+
coralRoller.hasCoral.onTrue(coralRoller.stop());
447454

448455
coralRoller.isRolling.or(algaeRoller.isRolling).whileTrue(createRollerAnimationCommand());
449456
}

‎src/main/java/frc/robot/auto/BlueL4Auto.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ public AutoRoutine getAutoRoutine() {
5252
blueCenterToL4G.done().onTrue(
5353
Commands.sequence(
5454
Commands.waitSeconds(1.0),
55-
coralRoller.createOuttakeCommand().withTimeout(0.2)));
55+
coralRoller.outtakeToL4().withTimeout(0.2)));
5656

5757
// spotless:on
5858

‎src/main/java/frc/robot/auto/BlueNoProcess3PieceAuto.java

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ public AutoRoutine getAutoRoutine() {
5151
blueNoProcessToL4I.done().onTrue(
5252
Commands.sequence(
5353
Commands.waitSeconds(0.1),
54-
coralRoller.createOuttakeCommand().withTimeout(0.2),
54+
coralRoller.outtakeToL4().withTimeout(0.2),
5555
Commands.parallel(
5656
blueL4IToSource.cmd(),
5757
elevator.coralIntakeCG())));
@@ -66,7 +66,7 @@ public AutoRoutine getAutoRoutine() {
6666
blueSourceToL4L.done().onTrue(
6767
Commands.sequence(
6868
Commands.waitSeconds(0.1),
69-
coralRoller.createOuttakeCommand().withTimeout(0.2),
69+
coralRoller.outtakeToL4().withTimeout(0.2),
7070
Commands.parallel(
7171
elevator.coralIntakeCG(),
7272
blueL4LToSource.cmd())));
@@ -81,7 +81,7 @@ public AutoRoutine getAutoRoutine() {
8181
blueSourceToL4K.done().onTrue(
8282
Commands.sequence(
8383
Commands.waitSeconds(0.1),
84-
coralRoller.createOuttakeCommand()));
84+
coralRoller.outtakeToL4()));
8585
// spotless:on
8686

8787
return blueNoProcess3PieceRoutine;

‎src/main/java/frc/robot/auto/BlueProcess3PieceAuto.java

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ public AutoRoutine getAutoRoutine() {
5050
Commands.waitSeconds(0.1),
5151
elevator.coralL4PositionCG(),
5252
Commands.waitSeconds(0.1),
53-
coralRoller.createOuttakeCommand().withTimeout(0.2),
53+
coralRoller.outtakeToL4().withTimeout(0.2),
5454
Commands.parallel(
5555
blueL4FToSource.cmd(),
5656
Commands.sequence(
@@ -59,15 +59,15 @@ public AutoRoutine getAutoRoutine() {
5959

6060
blueL4FToSource.done().onTrue(
6161
Commands.sequence(
62-
coralRoller.createIntakeCommand().withTimeout(0.2),
62+
coralRoller.intake().withTimeout(0.2),
6363
blueSourceToL4D.cmd()));
6464

6565
blueSourceToL4D.done().onTrue(
6666
Commands.sequence(
6767
Commands.waitSeconds(0.1),
6868
elevator.coralL4PositionCG(),
6969
Commands.waitSeconds(0.1),
70-
coralRoller.createOuttakeCommand().withTimeout(0.2),
70+
coralRoller.outtakeToL4().withTimeout(0.2),
7171
Commands.parallel(
7272
blueL4DToSource.cmd(),
7373
Commands.sequence(
@@ -77,15 +77,15 @@ public AutoRoutine getAutoRoutine() {
7777

7878
blueL4DToSource.done().onTrue(
7979
Commands.sequence(
80-
coralRoller.createIntakeCommand().withTimeout(0.2),
80+
coralRoller.intake().withTimeout(0.2),
8181
elevator.coralL4PositionCG()));
8282

8383
blueSourceToL4C.done().onTrue(
8484
Commands.sequence(
8585
Commands.waitSeconds(0.1),
8686
elevator.coralL4PositionCG(),
8787
Commands.waitSeconds(0.1),
88-
coralRoller.createOuttakeCommand()));
88+
coralRoller.outtakeToL4()));
8989
// spotless:on
9090

9191
return blueProcess3PieceRoutine;

‎src/main/java/frc/robot/auto/RedL4Auto.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ public AutoRoutine getAutoRoutine() {
5252
redCenterToL4G.done().onTrue(
5353
Commands.sequence(
5454
Commands.waitSeconds(1.0),
55-
coralRoller.createOuttakeCommand().withTimeout(0.2)));
55+
coralRoller.outtakeToL4().withTimeout(0.2)));
5656

5757
// spotless:on
5858

‎src/main/java/frc/robot/auto/RedNoProcess3PieceAuto.java

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ public AutoRoutine getAutoRoutine() {
5151
redNoProcessToL4I.done().onTrue(
5252
Commands.sequence(
5353
Commands.waitSeconds(0.1),
54-
coralRoller.createOuttakeCommand().withTimeout(0.2),
54+
coralRoller.outtakeToL4().withTimeout(0.2),
5555
Commands.parallel(
5656
redL4IToSource.cmd(),
5757
elevator.coralIntakeCG())));
@@ -66,7 +66,7 @@ public AutoRoutine getAutoRoutine() {
6666
redSourceToL4L.done().onTrue(
6767
Commands.sequence(
6868
Commands.waitSeconds(0.1),
69-
coralRoller.createOuttakeCommand().withTimeout(0.2),
69+
coralRoller.outtakeToL4().withTimeout(0.2),
7070
Commands.parallel(
7171
elevator.coralIntakeCG(),
7272
redL4LToSource.cmd())));
@@ -81,7 +81,7 @@ public AutoRoutine getAutoRoutine() {
8181
redSourceToL4K.done().onTrue(
8282
Commands.sequence(
8383
Commands.waitSeconds(0.1),
84-
coralRoller.createOuttakeCommand()));
84+
coralRoller.outtakeToL4()));
8585
// spotless:on
8686

8787
return redNoProcess3PieceRoutine;

‎src/main/java/frc/robot/auto/RedProcess3PieceAuto.java

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ public AutoRoutine getAutoRoutine() {
5151
redCenterToL4F.done().onTrue(
5252
Commands.sequence(
5353
Commands.waitSeconds(0.1),
54-
coralRoller.createOuttakeCommand().withTimeout(0.2),
54+
coralRoller.outtakeToL4().withTimeout(0.2),
5555
Commands.parallel(
5656
redL4FToSource.cmd(),
5757
elevator.coralIntakeCG())));
@@ -66,7 +66,7 @@ public AutoRoutine getAutoRoutine() {
6666
redSourceToL4D.done().onTrue(
6767
Commands.sequence(
6868
Commands.waitSeconds(0.1),
69-
coralRoller.createOuttakeCommand().withTimeout(0.2),
69+
coralRoller.outtakeToL4().withTimeout(0.2),
7070
Commands.parallel(
7171
elevator.coralIntakeCG(),
7272
redL4DToSource.cmd())));
@@ -81,7 +81,7 @@ public AutoRoutine getAutoRoutine() {
8181
redSourceToL4C.done().onTrue(
8282
Commands.sequence(
8383
Commands.waitSeconds(0.1),
84-
coralRoller.createOuttakeCommand()));
84+
coralRoller.outtakeToL4()));
8585
// spotless:on
8686

8787
return redProcess3PieceRoutine;

‎src/main/java/frc/robot/elevator/AlgaeRoller.java

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ public AlgaeRoller() {
6161
followerMotor.configureAsync(
6262
followerConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
6363

64-
setDefaultCommand(createStopCommand());
64+
setDefaultCommand(stop());
6565
}
6666

6767
@Override
@@ -81,25 +81,25 @@ public double getRollerVelocity() {
8181
return encoder.getVelocity();
8282
}
8383

84-
public Command createStopCommand() {
84+
public Command stop() {
8585
return this.startEnd(() -> leaderMotor.set(0.0), () -> {}).withName("Stop");
8686
}
8787

88-
public Command createIntakeCommand() {
88+
public Command intake() {
8989
return this.run(() -> setVoltage(AlgaeRollerConstants.kIntakeVoltage)).withName("Intake");
9090
}
9191

92-
public Command createOuttakeToBargeCommand() {
92+
public Command outtakeToBarge() {
9393
return this.run(() -> setVoltage(AlgaeRollerConstants.kOuttakeToBargeVoltage))
9494
.withName("Outtake to barge");
9595
}
9696

97-
public Command createOuttakeToProcessorCommand() {
97+
public Command outtakeToProcessor() {
9898
return this.run(() -> setVoltage(AlgaeRollerConstants.kOuttakeToProcessorVoltage))
9999
.withName("Outtake to processor");
100100
}
101101

102-
public Command createHoldAlgaeCommand() {
102+
public Command holdAlgae() {
103103
return this.run(() -> setVoltage(AlgaeRollerConstants.kHoldVoltage)).withName("Hold");
104104
}
105105
}

‎src/main/java/frc/robot/elevator/CoralRoller.java

Lines changed: 23 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ public CoralRoller() {
5050

5151
motor.configureAsync(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
5252

53-
setDefaultCommand(createStopCommand());
53+
setDefaultCommand(stop());
5454
}
5555

5656
@Override
@@ -70,15 +70,31 @@ public double getRollerVelocity() {
7070
return encoder.getVelocity();
7171
}
7272

73-
public Command createStopCommand() {
74-
return this.startEnd(() -> motor.set(0.0), () -> {});
73+
public Command stop() {
74+
return this.startEnd(() -> motor.set(0.0), () -> {}).withName("Stop");
7575
}
7676

77-
public Command createIntakeCommand() {
78-
return this.run(() -> setVoltage(CoralRollerConstants.kIntakeVoltage));
77+
public Command intake() {
78+
return this.run(() -> setVoltage(CoralRollerConstants.kIntakeVoltage)).withName("Intake");
7979
}
8080

81-
public Command createOuttakeCommand() {
82-
return this.run(() -> setVoltage(CoralRollerConstants.kOuttakeVoltage));
81+
public Command outtakeToL1() {
82+
return this.run(() -> setVoltage(CoralRollerConstants.kOuttakeToL1Voltage))
83+
.withName("Outtake to L1");
84+
}
85+
86+
public Command outtakeToL2() {
87+
return this.run(() -> setVoltage(CoralRollerConstants.kOuttakeToL2Voltage))
88+
.withName("Outtake to L2");
89+
}
90+
91+
public Command outtakeToL3() {
92+
return this.run(() -> setVoltage(CoralRollerConstants.kOuttakeToL3Voltage))
93+
.withName("Outtake to L3");
94+
}
95+
96+
public Command outtakeToL4() {
97+
return this.run(() -> setVoltage(CoralRollerConstants.kOuttakeToL4Voltage))
98+
.withName("Outtake to L4");
8399
}
84100
}

‎src/main/java/frc/robot/elevator/Elevator.java

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,7 @@ public Command coralIntakeCG() {
101101
lifter.setHeight(LifterState.CoralIntake).schedule();
102102
coralWrist.setAngle(CoralWristState.Intake).schedule();
103103
algaeWrist.setAngle(AlgaeWristState.CoralMode).schedule();
104-
coralRoller.createIntakeCommand().schedule();
104+
coralRoller.intake().schedule();
105105
algaeRoller.getCurrentCommand().cancel();
106106
});
107107
}
@@ -123,7 +123,7 @@ public Command algaeL3IntakeCG() {
123123
coralWrist.setAngle(CoralWristState.AlgaeMode).schedule();
124124
algaeWrist.setAngle(AlgaeWristState.L3).schedule();
125125
coralRoller.getCurrentCommand().cancel();
126-
algaeRoller.createIntakeCommand().schedule();
126+
algaeRoller.intake().schedule();
127127
});
128128
}
129129

@@ -134,7 +134,7 @@ public Command algaeL2IntakeCG() {
134134
coralWrist.setAngle(CoralWristState.AlgaeMode).schedule();
135135
algaeWrist.setAngle(AlgaeWristState.L2).schedule();
136136
coralRoller.getCurrentCommand().cancel();
137-
algaeRoller.createIntakeCommand().schedule();
137+
algaeRoller.intake().schedule();
138138
});
139139
}
140140

@@ -155,14 +155,14 @@ public Command algaeFloorIntakeCG() {
155155
coralWrist.setAngle(CoralWristState.AlgaeMode).schedule();
156156
algaeWrist.setAngle(AlgaeWristState.Floor).schedule();
157157
coralRoller.getCurrentCommand().cancel();
158-
algaeRoller.createIntakeCommand().schedule();
158+
algaeRoller.intake().schedule();
159159
});
160160
}
161161

162162
public Command holdAlgaeCG() {
163163
return new InstantCommand(
164164
() -> {
165-
algaeRoller.createHoldAlgaeCommand().schedule();
165+
algaeRoller.holdAlgae().schedule();
166166
coralWrist.setAngle(CoralWristState.AlgaeMode).schedule();
167167
algaeWrist.setAngle(AlgaeWristState.Barge).schedule();
168168
});

0 commit comments

Comments
 (0)