Skip to content

Commit

Permalink
update 123 and 321 to use new trigger for intake
Browse files Browse the repository at this point in the history
  • Loading branch information
@legoguy1000
legoguy1000 committed Sep 27, 2024
1 parent 0628a4f commit d6d16d9
Show file tree
Hide file tree
Showing 2 changed files with 22 additions and 44 deletions.
33 changes: 11 additions & 22 deletions src/main/java/frc/robot/autos/P123.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,11 @@
import java.util.function.BooleanSupplier;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathPlannerPath;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.lib.util.FieldConstants;
import frc.robot.Constants;
import frc.robot.RobotContainer;
Expand Down Expand Up @@ -61,11 +59,7 @@ public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo
Command followPath5 = AutoBuilder.followPath(path5);
Command followPath6 = AutoBuilder.followPath(path6);

Trigger noteInIndexer = new Trigger(() -> this.intake.getIndexerBeamBrakeStatus())
.debounce(0.25, Debouncer.DebounceType.kRising);
Trigger noteInIntake = new Trigger(() -> this.intake.getintakeBeamBrakeStatus())
.debounce(0.25, Debouncer.DebounceType.kRising);
BooleanSupplier abort = () -> !noteInIndexer.getAsBoolean() && !noteInIntake.getAsBoolean();
BooleanSupplier abort = intake.noteInIndexer.negate().and(intake.noteInIntake.negate());

Command resetPosition = Commands.runOnce(() -> {
Pose2d initialState =
Expand All @@ -79,26 +73,21 @@ public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo
elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT,
Rotation2d.fromDegrees(36.5)).withTimeout(1.0))
.andThen(CommandFactory.Auto.runIndexer(intake));
SequentialCommandGroup part2 = followPath2
.alongWith(CommandFactory.intakeNote(intake), elevatorWrist
.goToPosition(elevatorHeight, Rotation2d.fromDegrees(38.5)).withTimeout(.5))
SequentialCommandGroup part2 = followPath2.alongWith(elevatorWrist
.goToPosition(elevatorHeight, Rotation2d.fromDegrees(38.5)).withTimeout(.5))
.andThen(CommandFactory.Auto.runIndexer(intake));
SequentialCommandGroup part3 = followPath3
.alongWith(CommandFactory.intakeNote(intake), elevatorWrist
.goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.5)).withTimeout(.5))
SequentialCommandGroup part3 = followPath3.alongWith(elevatorWrist
.goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.5)).withTimeout(.5))
.andThen(CommandFactory.Auto.runIndexer(intake));
SequentialCommandGroup part4 = followPath4
.alongWith(CommandFactory.intakeNote(intake),
elevatorWrist.goToPosition(elevatorHeight, Rotation2d.fromDegrees(33.0))
.withTimeout(.5))
.alongWith(elevatorWrist.goToPosition(elevatorHeight, Rotation2d.fromDegrees(33.0))
.withTimeout(.5))
.andThen(CommandFactory.Auto.runIndexer(intake))
.andThen(elevatorWrist.homePosition().withTimeout(.5));
Command part5 = followPath5.deadlineWith(CommandFactory.intakeNote(intake)).andThen(
Commands.either(Commands.none(), Commands.sequence(CommandFactory.intakeNote(intake),
CommandFactory.Auto.runIndexer(intake)), abort));
Command part6 = followPath6.deadlineWith(CommandFactory.intakeNote(intake)).andThen(
Commands.either(Commands.none(), Commands.sequence(CommandFactory.intakeNote(intake),
CommandFactory.Auto.runIndexer(intake)), abort));
Command part5 = followPath5.andThen(Commands.either(Commands.none(), Commands.sequence(
CommandFactory.intakeNote(intake), CommandFactory.Auto.runIndexer(intake)), abort));
Command part6 = followPath6.andThen(Commands.either(Commands.none(), Commands.sequence(
CommandFactory.intakeNote(intake), CommandFactory.Auto.runIndexer(intake)), abort));
Command midline = Commands.either(Commands.sequence(part5, part6), Commands.none(),
() -> RobotContainer.goToCenter.getEntry().getBoolean(false));

Expand Down
33 changes: 11 additions & 22 deletions src/main/java/frc/robot/autos/P321.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,11 @@
import java.util.function.BooleanSupplier;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathPlannerPath;
import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.lib.util.FieldConstants;
import frc.robot.Constants;
import frc.robot.RobotContainer;
Expand Down Expand Up @@ -58,11 +56,7 @@ public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo
Command followPath5 = AutoBuilder.followPath(path5);
Command followPath6 = AutoBuilder.followPath(path6);

Trigger noteInIndexer = new Trigger(() -> this.intake.getIndexerBeamBrakeStatus())
.debounce(0.25, Debouncer.DebounceType.kRising);
Trigger noteInIntake = new Trigger(() -> this.intake.getintakeBeamBrakeStatus())
.debounce(0.25, Debouncer.DebounceType.kRising);
BooleanSupplier abort = () -> !noteInIndexer.getAsBoolean() && !noteInIntake.getAsBoolean();
BooleanSupplier abort = intake.noteInIndexer.negate().and(intake.noteInIntake.negate());


Command resetPosition = Commands.runOnce(() -> {
Expand All @@ -77,26 +71,21 @@ public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo
elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT,
Rotation2d.fromDegrees(39.0)).withTimeout(1.0))
.andThen(CommandFactory.Auto.runIndexer(intake));
SequentialCommandGroup part2 = followPath2
.alongWith(CommandFactory.intakeNote(intake), elevatorWrist
.goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.0)).withTimeout(.5))
SequentialCommandGroup part2 = followPath2.alongWith(elevatorWrist
.goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.0)).withTimeout(.5))
.andThen(CommandFactory.Auto.runIndexer(intake));
SequentialCommandGroup part3 = followPath3
.alongWith(CommandFactory.intakeNote(intake), elevatorWrist
.goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.5)).withTimeout(.5))
SequentialCommandGroup part3 = followPath3.alongWith(elevatorWrist
.goToPosition(elevatorHeight, Rotation2d.fromDegrees(37.5)).withTimeout(.5))
.andThen(CommandFactory.Auto.runIndexer(intake));
SequentialCommandGroup part4 = followPath4
.alongWith(CommandFactory.intakeNote(intake),
elevatorWrist.goToPosition(elevatorHeight, Rotation2d.fromDegrees(36.0))
.withTimeout(.5))
.alongWith(elevatorWrist.goToPosition(elevatorHeight, Rotation2d.fromDegrees(36.0))
.withTimeout(.5))
.andThen(CommandFactory.Auto.runIndexer(intake))
.andThen(elevatorWrist.homePosition().withTimeout(.5));
Command part5 = followPath5.deadlineWith(CommandFactory.intakeNote(intake)).andThen(
Commands.either(Commands.none(), Commands.sequence(CommandFactory.intakeNote(intake),
CommandFactory.Auto.runIndexer(intake)), abort));
Command part6 = followPath6.deadlineWith(CommandFactory.intakeNote(intake)).andThen(
Commands.either(Commands.none(), Commands.sequence(CommandFactory.intakeNote(intake),
CommandFactory.Auto.runIndexer(intake)), abort));
Command part5 = followPath5.andThen(Commands.either(Commands.none(), Commands.sequence(
CommandFactory.intakeNote(intake), CommandFactory.Auto.runIndexer(intake)), abort));
Command part6 = followPath6.andThen(Commands.either(Commands.none(), Commands.sequence(
CommandFactory.intakeNote(intake), CommandFactory.Auto.runIndexer(intake)), abort));
Command midline = Commands.either(Commands.sequence(part5, part6), Commands.none(),
() -> RobotContainer.goToCenter.getEntry().getBoolean(false));

Expand Down

0 comments on commit d6d16d9

Please sign in to comment.