Skip to content

Commit

Permalink
Remove the dead code
Browse files Browse the repository at this point in the history
  • Loading branch information
SCool62 committed Nov 17, 2024
1 parent 8fad9b3 commit 589b9d7
Showing 1 changed file with 0 additions and 80 deletions.
80 changes: 0 additions & 80 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -699,86 +699,6 @@ private Command staticAutoAim(double tolerance) {
return staticAutoAim(() -> tolerance);
}

private Command autoStaticAutoAim() {
return feeder
.indexCmd()
.deadlineWith(
swerve.runVelocityCmd(() -> new ChassisSpeeds()),
Commands.repeatingSequence(
shooter
.runFlywheelsCmd(() -> 0.0, () -> 0.0)
.unless(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0)
.until(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0),
shooter.runStateCmd(
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(),
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(),
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS())))
.until(() -> feeder.getFirstBeambreak())
.unless(() -> feeder.getFirstBeambreak())
.withTimeout(1.0)
.andThen(
staticAutoAim(6.0)
.deadlineWith(
intake.runVoltageCmd(0, 0).asProxy(), carriage.runVoltageCmd(0).asProxy())
.withTimeout(2.0)
.unless(() -> !feeder.getFirstBeambreak()))
.asProxy()
.withTimeout(4.0);
}

private Command autoFenderShot() {
return shooter
.runStateCmd(
AutoAim.FENDER_SHOT.getRotation(),
AutoAim.FENDER_SHOT.getLeftRPS(),
AutoAim.FENDER_SHOT.getRightRPS())
.raceWith(
feeder
.runVelocityCmd(0.0)
.until(() -> shooter.isAtGoal())
.andThen(
feeder
.runVelocityCmd(FeederSubsystem.INDEXING_VELOCITY)
.raceWith(
Commands.waitUntil(() -> !feeder.getFirstBeambreak())
.andThen(Commands.waitSeconds(0.1)))))
.asProxy();
}

private Command autoIntake() {
return Commands.parallel(
intake
.runVelocityCmd(50.0, 30.0)
.until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak())
.asProxy(),
feeder.indexCmd().asProxy(),
carriage
.runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE)
.until(() -> feeder.getFirstBeambreak())
.asProxy(),
shooter
.runStateCmd(
() ->
Rotation2d.fromDegrees(
MathUtil.clamp(
AutoAim.shotMap
.get(swerve.getDistanceToSpeaker())
.getRotation()
.getDegrees(),
0,
20)),
() -> 0.0,
() -> 0.0)
.until(() -> feeder.getFirstBeambreak())
.unless(() -> feeder.getFirstBeambreak())
.andThen(
shooter.runStateCmd(
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(),
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(),
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS()))
.asProxy());
}

private Command ampHeadingSnap(DoubleSupplier x, DoubleSupplier y) {
var headingController =
new ProfiledPIDController(
Expand Down

0 comments on commit 589b9d7

Please sign in to comment.