Skip to content

Commit ec764d9

Browse files
Nummun14Nummun14
committed
Added algae collection states and unload coral command
1 parent 6842f2c commit ec764d9

File tree

6 files changed

+71
-24
lines changed

6 files changed

+71
-24
lines changed

‎src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ public class AlgaeManipulationCommands {
2828

2929
public static Command getAlgaeCollectionCommandCommand() {
3030
return new SequentialCommandGroup(
31+
CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece),
3132
getInitiateAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece),
3233
new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand()) //TODO: add coral unloading if needed
3334
).alongWith(getAlignToReefCommand());
@@ -47,7 +48,7 @@ private static Command getAlignToReefCommand() {
4748
).raceWith(
4849
new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.ARM::hasGamePiece)),
4950
new WaitUntilCommand(OperatorConstants.STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER)
50-
); // TODO: .onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy();
51+
).onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy();
5152
}
5253

5354
private static Command getScoreAlgaeCommand() {
@@ -112,8 +113,8 @@ private static int getAlgaeScoreMethodSelector() {
112113

113114
private static Command getInitiateAlgaeCollectionCommand() {
114115
return new ParallelCommandGroup(
115-
ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.COLLECT_ALGAE_FROM_REEF), //TODO: make this depend on the target scoring level
116-
ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FROM_L3)
116+
ArmCommands.getSetTargetStateCommand(CoralPlacingCommands.REEF_CHOOSER.getArmAlgaeCollectionState()),
117+
ElevatorCommands.getSetTargetStateCommand(CoralPlacingCommands.REEF_CHOOSER.getElevatorAlgaeCollectionState())
117118
);
118119
}
119120

‎src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,13 @@ public static Command getLoadCoralCommand() {
2626
).until(RobotContainer.ARM::hasGamePiece);
2727
}
2828

29+
public static Command getUnloadCoralCommand() {
30+
return new ParallelCommandGroup(
31+
ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.UNLOAD_CORAL),
32+
ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.UNLOAD_CORAL)
33+
).until(() -> !RobotContainer.ARM.hasGamePiece());
34+
}
35+
2936
public static Command getCoralCollectionCommand() {
3037
return new ParallelCommandGroup(
3138
getIntakeSequenceCommand(),

‎src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java‎

Lines changed: 39 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@
2323

2424
public class CoralPlacingCommands {
2525
public static boolean SHOULD_SCORE_AUTONOMOUSLY = true;
26-
private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER;
26+
static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER;
2727

2828
public static Command getScoreInReefCommand(boolean shouldScoreRight) {
2929
return new ConditionalCommand(
@@ -37,8 +37,8 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) {
3737
return new SequentialCommandGroup(
3838
getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isArmAndElevatorAtPrepareState(shouldScoreRight)),
3939
new ParallelCommandGroup(
40-
ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState),
41-
ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore)
40+
ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorCoralState),
41+
ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmCoralState, CoralPlacingCommands::shouldReverseScore)
4242
)
4343
);
4444
}
@@ -47,16 +47,16 @@ private static Command getScoreCommand(boolean shouldScoreRight) {
4747
return new SequentialCommandGroup(
4848
getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER),
4949
new ParallelCommandGroup(
50-
ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState),
51-
ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore)
50+
ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorCoralState),
51+
ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmCoralState, CoralPlacingCommands::shouldReverseScore)
5252
)
5353
);
5454
}
5555

5656
private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) {
5757
return new ParallelCommandGroup(
58-
ElevatorCommands.getPrepareStateCommand(REEF_CHOOSER::getElevatorState),
59-
ArmCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore),
58+
ElevatorCommands.getPrepareStateCommand(REEF_CHOOSER::getElevatorCoralState),
59+
ArmCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmCoralState, CoralPlacingCommands::shouldReverseScore),
6060
getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).asProxy()
6161
);
6262
}
@@ -134,8 +134,12 @@ public enum ScoringLevel {
134134
L3(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)),
135135
L4(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0));
136136

137-
public final ElevatorConstants.ElevatorState elevatorState;
138-
public final ArmConstants.ArmState armState;
137+
public final ElevatorConstants.ElevatorState
138+
elevatorCoralState,
139+
elevatorAlgaeCollectionState;
140+
public final ArmConstants.ArmState
141+
armCoralState,
142+
armAlgaeCollectionState;
139143
public final int level = calculateLevel();
140144
final double xTransformMeters, positiveYTransformMeters;
141145
final Rotation2d rotationTransform;
@@ -153,8 +157,10 @@ public enum ScoringLevel {
153157
this.xTransformMeters = xTransformMeters;
154158
this.positiveYTransformMeters = positiveYTransformMeters;
155159
this.rotationTransform = rotationTransform;
156-
this.elevatorState = determineElevatorState();
157-
this.armState = determineArmState();
160+
this.elevatorCoralState = determineElevatorCoralState();
161+
this.elevatorAlgaeCollectionState = determineElevatorAlgaeCollectionState();
162+
this.armCoralState = determineArmCoralState();
163+
this.armAlgaeCollectionState = determineArmAlgaeCollectionState();
158164
}
159165

160166
/**
@@ -176,7 +182,7 @@ public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPo
176182
return new FlippablePose2d(reefCenterPose.plus(transform), true);
177183
}
178184

179-
private ElevatorConstants.ElevatorState determineElevatorState() {
185+
private ElevatorConstants.ElevatorState determineElevatorCoralState() {
180186
return switch (level) {
181187
case 1 -> ElevatorConstants.ElevatorState.SCORE_L1;
182188
case 2 -> ElevatorConstants.ElevatorState.SCORE_L2;
@@ -186,7 +192,17 @@ private ElevatorConstants.ElevatorState determineElevatorState() {
186192
};
187193
}
188194

189-
private ArmConstants.ArmState determineArmState() {
195+
private ElevatorConstants.ElevatorState determineElevatorAlgaeCollectionState() {
196+
return switch (level) {
197+
case 1 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_GROUND;
198+
case 2 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_L2;
199+
case 3 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_L3;
200+
case 4 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP;
201+
default -> throw new IllegalStateException("Unexpected value: " + ordinal());
202+
};
203+
}
204+
205+
private ArmConstants.ArmState determineArmCoralState() {
190206
return switch (level) {
191207
case 1 -> ArmConstants.ArmState.SCORE_L1;
192208
case 2 -> ArmConstants.ArmState.SCORE_L2;
@@ -196,6 +212,16 @@ private ArmConstants.ArmState determineArmState() {
196212
};
197213
}
198214

215+
private ArmConstants.ArmState determineArmAlgaeCollectionState() {
216+
return switch (level) {
217+
case 1 -> ArmConstants.ArmState.COLLECT_ALGAE_FLOOR;
218+
case 2 -> ArmConstants.ArmState.COLLECT_ALGAE_L2;
219+
case 3 -> ArmConstants.ArmState.COLLECT_ALGAE_L3;
220+
case 4 -> ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP;
221+
default -> throw new IllegalStateException("Unexpected value: " + ordinal());
222+
};
223+
}
224+
199225
private int calculateLevel() {
200226
return ordinal() + 1;
201227
}

‎src/main/java/frc/trigon/robot/misc/ReefChooser.java‎

Lines changed: 13 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,8 @@
77
import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands;
88
import frc.trigon.robot.constants.FieldConstants;
99
import frc.trigon.robot.constants.OperatorConstants;
10-
import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
1110
import frc.trigon.robot.subsystems.arm.ArmConstants;
11+
import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
1212
import lib.utilities.flippable.FlippablePose2d;
1313

1414
import java.util.function.Supplier;
@@ -45,12 +45,20 @@ public FlippablePose2d calculateTargetScoringPose() {
4545
return scoringLevel.calculateTargetPlacingPosition(clockPosition, reefSide);
4646
}
4747

48-
public ArmConstants.ArmState getArmState() {
49-
return scoringLevel.armState;
48+
public ArmConstants.ArmState getArmCoralState() {
49+
return scoringLevel.armCoralState;
50+
}
51+
52+
public ArmConstants.ArmState getArmAlgaeCollectionState() {
53+
return scoringLevel.armAlgaeCollectionState;
54+
}
55+
56+
public ElevatorConstants.ElevatorState getElevatorCoralState() {
57+
return scoringLevel.elevatorCoralState;
5058
}
5159

52-
public ElevatorConstants.ElevatorState getElevatorState() {
53-
return scoringLevel.elevatorState;
60+
public ElevatorConstants.ElevatorState getElevatorAlgaeCollectionState() {
61+
return scoringLevel.elevatorAlgaeCollectionState;
5462
}
5563

5664
private void configureBindings() {

‎src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -235,6 +235,7 @@ public enum ArmState {
235235
REST_WITH_CORAL(Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180), 0),
236236
REST_FOR_CLIMB(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0),
237237
LOAD_CORAL(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), -4),
238+
UNLOAD_CORAL(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 4),
238239
HOLD_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4),
239240
EJECT(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(60), 4),
240241
SCORE_L1(Rotation2d.fromDegrees(110), Rotation2d.fromDegrees(110), 4),
@@ -243,8 +244,10 @@ public enum ArmState {
243244
SCORE_L4(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4),
244245
SCORE_NET(Rotation2d.fromDegrees(160), Rotation2d.fromDegrees(160), 4),
245246
SCORE_PROCESSOR(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4),
247+
COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), -4),
246248
COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4),
247249
COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4),
250+
COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(45), Rotation2d.fromDegrees(45), -4),
248251
EJECT_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4);
249252

250253
public final Rotation2d targetAngle;

‎src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -175,13 +175,15 @@ private static void configureReverseLimitSensor() {
175175
public enum ElevatorState {
176176
REST(0.603, 0.603, 0.7),
177177
LOAD_CORAL(0.5519, 0.5519, 0.7),
178+
UNLOAD_CORAL(0.5519, 0.5519, 0.7),
178179
SCORE_L1(0.1, 0.1, 1),
179180
SCORE_L2(0.3, 0.4, 1),
180181
SCORE_L3(0.7, 0.8, 1),
181182
SCORE_L4(1.2, 1.3, 1),
182-
COLLECT_ALGAE_FROM_L2(0.603, 0.603, 1),
183-
COLLECT_ALGAE_FROM_L3(0.953, 0.953, 1),
184-
COLLECT_ALGAE_FROM_GROUND(0, 0, 0.7),
183+
COLLECT_ALGAE_L2(0.603, 0.603, 1),
184+
COLLECT_ALGAE_L3(0.953, 0.953, 1),
185+
COLLECT_ALGAE_GROUND(0, 0, 1),
186+
COLLECT_ALGAE_LOLLIPOP(0.3, 0.3, 1),
185187
REST_WITH_ALGAE(0.603, 0.603, 0.3),
186188
SCORE_NET(1.382, 1.382, 0.3),
187189
SCORE_PROCESSOR(1.182, 1.182, 0.3);

0 commit comments

Comments
 (0)