Skip to content

Commit 1b3152a

Browse files
only time out the second part of auto align in auto
1 parent 4e4b0a6 commit 1b3152a

File tree

1 file changed

+8
-5
lines changed

1 file changed

+8
-5
lines changed

src/main/java/team1403/robot/RobotContainer.java

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -177,15 +177,18 @@ private Command getAlignCommand(ReefSelect select) {
177177
case L4: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(2)); break;
178178
case drive: default: /* do nothing */ break;
179179
}
180-
}
180+
}
181+
182+
Command finalAlign = new AlignCommand(m_swerve, target);
183+
if (DriverStation.isAutonomous()) finalAlign = finalAlign.withTimeout(1.5);
181184

182185
if(CougarUtil.getDistance(target, m_swerve.getPose()) > 0.2)
183186
return Commands.sequence(
184187
AutoBuilder.pathfindToPose(target, TunerConstants.kAutoAlignConstraints),
185-
new AlignCommand(m_swerve, target)
188+
finalAlign
186189
);
187190
else
188-
return new AlignCommand(m_swerve, target);
191+
return finalAlign;
189192
}, Set.of(m_swerve)),
190193
Blackbox.setAligningCmd(false)
191194
).finallyDo((interrupted) -> {
@@ -416,8 +419,8 @@ private void configureBindings() {
416419
Commands.waitUntil(() -> Blackbox.isCoralLoaded()));
417420
NamedCommands.registerCommand("WaitForSetpoint",
418421
new WaitUntilDebounced(() -> m_wrist.isAtSetpoint() && m_elevator.isAtSetpoint(), 0.1).withTimeout(3));
419-
NamedCommands.registerCommand("ReefAlignL", getAlignCommand(Blackbox.ReefSelect.LEFT).withTimeout(2));
420-
NamedCommands.registerCommand("ReefAlignR", getAlignCommand(Blackbox.ReefSelect.RIGHT).withTimeout(2));
422+
NamedCommands.registerCommand("ReefAlignL", getAlignCommand(Blackbox.ReefSelect.LEFT));
423+
NamedCommands.registerCommand("ReefAlignR", getAlignCommand(Blackbox.ReefSelect.RIGHT));
421424
NamedCommands.registerCommand("Loading", Blackbox.robotStateCmd(Blackbox.State.loading));
422425

423426

0 commit comments

Comments
 (0)