Skip to content

Commit 726128b

Browse files
JetblackdragonRobototesProgrammerskoolpooloiamawesomecat
authored
Girls gen updates (#189)
* elastic camera fix * autoScore stuff * l1 auto align * ee * c * super jank code to get auto l1 auto extake * c * ds changes * quick hand off fix * log + spot * Code Clean up for get coral branch height * Removed auto score rubmle * Correct use of ipScore repeat prescoreswing * Added comments for the in place scoring * Consolidated all autoAligns into 1 * Added static classes to check for alliance side. * Removal of string checker. * spotless * name fix * AutoAlign Cleanup * AutoAlign Cleanup * isConnected fix avoids calling isConnected 3 different times. * Coral Levels refactoring * Change type to alignType for clarity --------- Co-authored-by: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Co-authored-by: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Co-authored-by: iamawesomecat <145405364+iamawesomecat@users.noreply.github.com>
1 parent eec781e commit 726128b

File tree

3 files changed

+156
-263
lines changed

3 files changed

+156
-263
lines changed

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

Lines changed: 16 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -170,12 +170,16 @@ private void configureDrivebaseBindings() {
170170
// applying the request to drive with the inputs
171171
s.drivebaseSubsystem
172172
.applyRequest(
173-
() ->
174-
drive
175-
.withVelocityX(soloController.isConnected() ? getSoloDriveX() : getDriveX())
176-
.withVelocityY(soloController.isConnected() ? getSoloDriveY() : getDriveY())
177-
.withRotationalRate(
178-
soloController.isConnected() ? getSoloDriveRotate() : getDriveRotate()))
173+
() -> {
174+
boolean soloConnected = soloController.isConnected();
175+
SwerveRequest.FieldCentric request =
176+
drive
177+
.withVelocityX(soloConnected ? getSoloDriveX() : getDriveX())
178+
.withVelocityY(soloConnected ? getSoloDriveY() : getDriveY())
179+
.withRotationalRate(
180+
soloConnected ? getSoloDriveRotate() : getDriveRotate());
181+
return request;
182+
})
179183
.withName("Drive"));
180184

181185
// various former controls that were previously used and could be referenced in the future
@@ -495,9 +499,12 @@ private Command getCoralBranchHeightCommand(ScoringType version) {
495499
} else {
496500
return switch (branchHeight) {
497501
case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper());
498-
case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper());
499-
case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper());
500-
case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper());
502+
case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(
503+
driverController.rightBumper(), () -> false);
504+
case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(
505+
driverController.rightBumper(), () -> false);
506+
case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(
507+
driverController.rightBumper(), () -> false);
501508
};
502509
}
503510
}

‎src/main/java/frc/robot/subsystems/SuperStructure.java‎

Lines changed: 52 additions & 150 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,32 @@
1414
import java.util.function.Supplier;
1515

1616
public class SuperStructure {
17+
private static class CoralLevelConfig {
18+
final String name;
19+
final double elevatorPrePos;
20+
final double armScore;
21+
final double preTimeout;
22+
23+
CoralLevelConfig(String name, double elevatorPrePos, double armScore, double preTimeout) {
24+
this.name = name;
25+
this.elevatorPrePos = elevatorPrePos;
26+
this.armScore = armScore;
27+
this.preTimeout = preTimeout;
28+
}
29+
}
30+
31+
private static final CoralLevelConfig L4 =
32+
new CoralLevelConfig(
33+
"L4", ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS, ArmPivot.CORAL_PRESET_PRE_L4, 0.7);
34+
35+
private static final CoralLevelConfig L3 =
36+
new CoralLevelConfig(
37+
"L3", ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS, ArmPivot.CORAL_PRESET_L3, 0.5);
38+
39+
private static final CoralLevelConfig L2 =
40+
new CoralLevelConfig(
41+
"L2", ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS, ArmPivot.CORAL_PRESET_L2, 0.5);
42+
1743
private final ElevatorSubsystem elevator;
1844
private final ArmPivot armPivot;
1945
private final SpinnyClaw spinnyClaw;
@@ -57,16 +83,6 @@ private Command colorSet(int r, int g, int b, String name) {
5783
return elevatorLight.colorSet(r, g, b, name);
5884
}
5985

60-
private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) {
61-
// repeats scoring sequence if the coral is still in the claw
62-
if (armSensor == null) {
63-
return Commands.sequence(
64-
command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(score));
65-
} else {
66-
return command.repeatedly().onlyWhile(armSensor.inClaw());
67-
}
68-
}
69-
7086
private Command repeatPrescoreScoreSwing(
7187
Command command, BooleanSupplier score, BooleanSupplier ipScore) {
7288
// repeats scoring sequence if the coral is still in the claw
@@ -75,172 +91,58 @@ private Command repeatPrescoreScoreSwing(
7591
command,
7692
Commands.waitUntil(() -> !score.getAsBoolean()),
7793
Commands.waitUntil(ipScore).until(score));
78-
} else {
79-
return command.repeatedly().onlyWhile(armSensor.inClaw());
8094
}
95+
return command.repeatedly().onlyWhile(armSensor.inClaw());
8196
}
8297

83-
public Command coralLevelFour(BooleanSupplier score) {
84-
if (branchSensors != null) { // checks if sensor enabled then use for faster scoring
98+
private Command coralLevel(
99+
CoralLevelConfig cfg,
100+
BooleanSupplier score,
101+
BooleanSupplier ipScore,
102+
boolean skipPreIntakeInAuto) {
103+
104+
if (branchSensors != null) {
85105
score = branchSensors.withinScoreRange().or(score);
86106
}
87-
return Commands.sequence(
88-
Commands.parallel(
89-
Commands.print("Pre position"),
90-
elevator
91-
.setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS)
92-
.deadlineFor( // keeps spinny claw engaged until coral has been scored
93-
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)),
94-
spinnyClaw.stop())
95-
.withTimeout(0.7),
96-
repeatPrescoreScoreSwing(
97-
Commands.sequence(
98-
Commands.parallel(
99-
elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS),
100-
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_PRE_L4))
101-
.withDeadline(
102-
Commands.waitUntil(
103-
score)), // waits until driver presses the score button or until
104-
// auto scoring happens
105-
armPivot
106-
.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
107-
.withTimeout(1.5)
108-
.until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
109-
score),
110-
Commands.print("Pre preIntake()"),
111-
coralPreIntake()
112-
.unless(
113-
RobotModeTriggers
114-
.autonomous()), // doesn't go to preintake if auto, should add for otehr
115-
// commands
116-
Commands.print("Post preIntake()"))
117-
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L4").asProxy())
118-
.withName("Coral Level 4");
119-
}
120107

121-
public Command coralLevelThree(BooleanSupplier score) { // same as L4
122108
return Commands.sequence(
123109
Commands.parallel(
124110
elevator
125-
.setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS)
126-
.deadlineFor(
127-
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)),
128-
spinnyClaw.stop())
129-
.withTimeout(0.5),
130-
repeatPrescoreScoreSwing(
131-
Commands.repeatingSequence(
132-
armPivot
133-
.moveToPosition(ArmPivot.CORAL_PRESET_L3)
134-
.withDeadline(Commands.waitUntil(score)),
135-
armPivot
136-
.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
137-
.withTimeout(1.5)
138-
.until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
139-
score),
140-
coralPreIntake())
141-
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy())
142-
.withName("Coral Level 3");
143-
}
144-
145-
public Command coralLevelTwo(BooleanSupplier score) { // same as L4 and L3
146-
return Commands.sequence(
147-
Commands.parallel(
148-
elevator
149-
.setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS)
150-
.deadlineFor(
151-
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)),
152-
spinnyClaw.stop())
153-
.withTimeout(0.5),
154-
repeatPrescoreScoreSwing(
155-
Commands.sequence(
156-
armPivot
157-
.moveToPosition(ArmPivot.CORAL_PRESET_L2)
158-
.withDeadline(Commands.waitUntil(score)),
159-
armPivot
160-
.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
161-
.withTimeout(1.5)
162-
.until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
163-
score),
164-
coralPreIntake())
165-
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy())
166-
.withName("Coral Level 2");
167-
}
168-
169-
public Command coralLevelOne(BooleanSupplier score) {
170-
return Commands.sequence(
171-
Commands.parallel(
172-
elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS),
173-
armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1),
174-
spinnyClaw.stop()) // holds coral without wearing flywheels
175-
.withTimeout(0.5)
176-
.withDeadline(Commands.waitUntil(score)),
177-
spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.25), // spits out coral
178-
Commands.waitSeconds(1), // Wait to clear the reef
179-
coralPreIntake())
180-
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy())
181-
.withName("Coral Level 1");
182-
}
183-
184-
// New versions with ipScore
185-
// if robot is in position for intermediate scoring, it can score early but if vision is dead
186-
// manual control still works
187-
// only used on solo controls
188-
189-
public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { // same as L4
190-
return Commands.sequence(
191-
Commands.parallel(
192-
elevator
193-
.setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS)
111+
.setLevel(cfg.elevatorPrePos)
194112
.deadlineFor(
195113
armPivot
196114
.moveToPosition(ArmPivot.CORAL_PRESET_UP)
197115
.until(ipScore)
198116
.until(score)),
199117
spinnyClaw.stop())
200-
.withTimeout(0.5),
118+
.withTimeout(cfg.preTimeout),
201119
repeatPrescoreScoreSwing(
202-
Commands.repeatingSequence(
120+
Commands.sequence(
203121
armPivot
204-
.moveToPosition(ArmPivot.CORAL_PRESET_L3)
122+
.moveToPosition(cfg.armScore)
205123
.withDeadline(Commands.waitUntil(ipScore).until(score)),
206124
armPivot
207125
.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
208126
.withTimeout(1.5)
209127
.until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
210128
score,
211129
ipScore),
212-
coralPreIntake())
213-
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy())
214-
.withName("Coral Level 3");
130+
coralPreIntake()
131+
.unless(() -> skipPreIntakeInAuto && RobotModeTriggers.autonomous().getAsBoolean()))
132+
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L" + cfg.name).asProxy())
133+
.withName("Coral Level " + cfg.name);
215134
}
216135

217-
public Command coralLevelTwo(
218-
BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 and L3
219-
return Commands.sequence(
220-
Commands.parallel(
221-
elevator
222-
.setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS)
223-
.deadlineFor(
224-
armPivot
225-
.moveToPosition(ArmPivot.CORAL_PRESET_UP)
226-
.until(ipScore)
227-
.until(score)),
228-
spinnyClaw.stop())
229-
.withTimeout(0.5),
230-
repeatPrescoreScoreSwing(
231-
Commands.sequence(
232-
armPivot
233-
.moveToPosition(ArmPivot.CORAL_PRESET_L2)
234-
.withDeadline(Commands.waitUntil(ipScore).until(score)),
235-
armPivot
236-
.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)
237-
.withTimeout(1.5)
238-
.until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))),
239-
score,
240-
ipScore),
241-
coralPreIntake())
242-
.deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy())
243-
.withName("Coral Level 2");
136+
public Command coralLevelFour(BooleanSupplier score) {
137+
return coralLevel(L4, score, () -> false, true);
138+
}
139+
140+
public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) {
141+
return coralLevel(L3, score, ipScore, false);
142+
}
143+
144+
public Command coralLevelTwo(BooleanSupplier score, BooleanSupplier ipScore) {
145+
return coralLevel(L2, score, ipScore, false);
244146
}
245147

246148
public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) {

0 commit comments

Comments
 (0)