Skip to content

Commit

Permalink
autos with new Indexer subsytem
Browse files Browse the repository at this point in the history
  • Loading branch information
@gvaldez7206
gvaldez7206 committed Sep 29, 2024
1 parent f484075 commit b595aa6
Show file tree
Hide file tree
Showing 11 changed files with 88 additions and 68 deletions.
9 changes: 5 additions & 4 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,11 +167,12 @@ public RobotContainer(RobotRunType runtimeType) {
}

autoChooser.setDefaultOption("Nothing", Commands.none());
autoChooser.addOption("P123", new P123(s_Swerve, elevatorWrist, intake, shooter));
autoChooser.addOption("P321", new P321(s_Swerve, elevatorWrist, intake, shooter));
autoChooser.addOption("P8765", new P8765(s_Swerve, elevatorWrist, intake, shooter));
autoChooser.addOption("P123", new P123(s_Swerve, elevatorWrist, intake, indexer, shooter));
autoChooser.addOption("P321", new P321(s_Swerve, elevatorWrist, intake, indexer, shooter));
autoChooser.addOption("P8765",
new P8765(s_Swerve, elevatorWrist, intake, indexer, shooter));
autoChooser.addOption("Just Shoot 1",
new JustShoot1(s_Swerve, elevatorWrist, intake, shooter));
new JustShoot1(s_Swerve, elevatorWrist, indexer, shooter));
// autoChooser.addOption("P32", new P32(s_Swerve, elevatorWrist, intake, shooter));
// autoChooser.addOption("P675", new P675(s_Swerve, elevatorWrist, intake, shooter));
// autoChooser.addOption("P3675", new P3675(s_Swerve, elevatorWrist, intake, shooter));
Expand Down
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/autos/JustShoot1.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
import frc.robot.Constants;
import frc.robot.commands.CommandFactory;
import frc.robot.subsystems.elevator_wrist.ElevatorWrist;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.indexer.Indexer;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.swerve.Swerve;

Expand All @@ -23,22 +23,22 @@ public class JustShoot1 extends SequentialCommandGroup {

Swerve swerveDrive;
ElevatorWrist elevatorWrist;
Intake intake;
Indexer indexer;
Shooter shooter;

/**
* P32 Auto
*
* @param swerveDrive Swerve Drive Subsystem
* @param elevatorWrist Elevator Wrist Subsystem
* @param intake Intake Subsystem
* @param indexer INdexer Subsystem
* @param shooter Shooter Subsystem
*/
public JustShoot1(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake,
public JustShoot1(Swerve swerveDrive, ElevatorWrist elevatorWrist, Indexer indexer,
Shooter shooter) {
this.swerveDrive = swerveDrive;
this.elevatorWrist = elevatorWrist;
this.intake = intake;
this.indexer = indexer;
this.shooter = shooter;

PathPlannerPath path1 = PathPlannerPath.fromChoreoTrajectory("just shoot 1");
Expand All @@ -52,7 +52,7 @@ public JustShoot1(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake
.plus(new Transform2d(0, 0, Rotation2d.fromDegrees(180))));
swerveDrive.resetOdometry(initialState);
});
SequentialCommandGroup part1 = followPath1.andThen(CommandFactory.Auto.runIndexer(intake));
SequentialCommandGroup part1 = followPath1.andThen(CommandFactory.Auto.runIndexer(indexer));

SequentialCommandGroup followPaths = part1;

Expand Down
29 changes: 19 additions & 10 deletions src/main/java/frc/robot/autos/P123.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import frc.robot.RobotContainer;
import frc.robot.commands.CommandFactory;
import frc.robot.subsystems.elevator_wrist.ElevatorWrist;
import frc.robot.subsystems.indexer.Indexer;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.swerve.Swerve;
Expand All @@ -25,6 +26,7 @@ public class P123 extends SequentialCommandGroup {
Swerve swerveDrive;
ElevatorWrist elevatorWrist;
Intake intake;
Indexer indexer;
Shooter shooter;

/**
Expand All @@ -33,12 +35,15 @@ public class P123 extends SequentialCommandGroup {
* @param swerveDrive Swerve Drive Subsystem
* @param elevatorWrist Elevator Wrist Subsystem
* @param intake Intake Subsystem
* @param indexer Indexer Subsystem
* @param shooter Shooter Subsystem
*/
public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shooter shooter) {
public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Indexer indexer,
Shooter shooter) {
this.swerveDrive = swerveDrive;
this.elevatorWrist = elevatorWrist;
this.intake = intake;
this.indexer = indexer;
this.shooter = shooter;
// addRequirements(swerveDrive);
// SmartDashboard.putBoolean("Auto Status", false);
Expand All @@ -59,7 +64,7 @@ public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo
Command followPath5 = AutoBuilder.followPath(path5);
Command followPath6 = AutoBuilder.followPath(path6);

BooleanSupplier abort = intake.noteInIndexer.negate().and(intake.noteInIntake.negate());
BooleanSupplier abort = indexer.noteInIndexer.negate().and(intake.noteInIntake.negate());

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

Expand Down
0 src/main/java/frc/robot/autos/P32.java → src/main/java/frc/robot/autos/P32.java.txt
View file
File renamed without changes.
29 changes: 19 additions & 10 deletions src/main/java/frc/robot/autos/P321.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import frc.robot.RobotContainer;
import frc.robot.commands.CommandFactory;
import frc.robot.subsystems.elevator_wrist.ElevatorWrist;
import frc.robot.subsystems.indexer.Indexer;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.swerve.Swerve;
Expand All @@ -25,6 +26,7 @@ public class P321 extends SequentialCommandGroup {
Swerve swerveDrive;
ElevatorWrist elevatorWrist;
Intake intake;
Indexer indexer;
Shooter shooter;

/**
Expand All @@ -33,12 +35,15 @@ public class P321 extends SequentialCommandGroup {
* @param swerveDrive Swerve Drive Subsystem
* @param elevatorWrist Elevator Wrist Subsystem
* @param intake Intake Subsystem
* @param indexer Indexer SUbsystem
* @param shooter Shooter Subsystem
*/
public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shooter shooter) {
public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Indexer indexer,
Shooter shooter) {
this.swerveDrive = swerveDrive;
this.elevatorWrist = elevatorWrist;
this.intake = intake;
this.indexer = indexer;
this.shooter = shooter;

PathPlannerPath path1 = PathPlannerPath.fromPathFile("1 - Resnick 2 Shoot Initial Note");
Expand All @@ -56,7 +61,7 @@ public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shoo
Command followPath5 = AutoBuilder.followPath(path5);
Command followPath6 = AutoBuilder.followPath(path6);

BooleanSupplier abort = intake.noteInIndexer.negate().and(intake.noteInIntake.negate());
BooleanSupplier abort = indexer.noteInIndexer.negate().and(intake.noteInIntake.negate());


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

Expand Down
0 src/main/java/frc/robot/autos/P3675.java → src/main/java/frc/robot/autos/P3675.java.txt
View file
File renamed without changes.
0 src/main/java/frc/robot/autos/P675.java → src/main/java/frc/robot/autos/P675.java.txt
View file
File renamed without changes.
77 changes: 39 additions & 38 deletions src/main/java/frc/robot/autos/P8765.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,19 +3,18 @@
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.math.geometry.Transform2d;
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;
import frc.robot.commands.CommandFactory;
import frc.robot.subsystems.elevator_wrist.ElevatorWrist;
import frc.robot.subsystems.indexer.Indexer;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.swerve.Swerve;
Expand All @@ -28,6 +27,7 @@ public class P8765 extends SequentialCommandGroup {
Swerve swerveDrive;
ElevatorWrist elevatorWrist;
Intake intake;
Indexer indexer;
Shooter shooter;

/**
Expand All @@ -36,12 +36,15 @@ public class P8765 extends SequentialCommandGroup {
* @param swerveDrive Swerve Drive Subsystem
* @param elevatorWrist Elevator Wrist Subsystem
* @param intake Intake Subsystem
* @param indexer Indexer Subsystem
* @param shooter Shooter Subsystem
*/
public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Shooter shooter) {
public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Indexer indexer,
Shooter shooter) {
this.swerveDrive = swerveDrive;
this.elevatorWrist = elevatorWrist;
this.intake = intake;
this.indexer = indexer;
this.shooter = shooter;

PathPlannerPath path0 = PathPlannerPath.fromChoreoTrajectory("P8765-initial");
Expand All @@ -65,65 +68,63 @@ public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Sho
Command followPath3_dump = AutoBuilder.followPath(path3_dump);
Command followPath4_dump = AutoBuilder.followPath(path4_dump);



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 =
() -> !indexer.noteInIndexer.getAsBoolean() && !intake.noteInIntake.getAsBoolean();
BooleanSupplier dumpOrNot = () -> RobotContainer.dumpNotes.getEntry().getBoolean(false);
double elevatorHeight = 28.0;
Command part0 = followPath0
.alongWith(
elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT,
Rotation2d.fromDegrees(31.0)).withTimeout(1.5))
.andThen(Commands.waitSeconds(.1)).andThen(CommandFactory.Auto.runIndexer(intake));
.andThen(Commands.waitSeconds(.1)).andThen(CommandFactory.Auto.runIndexer(indexer));
// .andThen(Commands.either(elevatorWrist.homePosition().withTimeout(.5), Commands.none(),
// dumpOrNot));
Command part1 = followPath1.deadlineWith(CommandFactory.intakeNote(intake),
Command part1 = followPath1.deadlineWith(CommandFactory.intakeNote(intake, indexer),
elevatorWrist.homePosition().withTimeout(1.0))
.andThen(Commands.either(Commands.none(),
Commands.sequence(CommandFactory.intakeNote(intake).alongWith(elevatorWrist
Commands.sequence(CommandFactory.intakeNote(intake, indexer).alongWith(elevatorWrist
.followPosition(() -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT,
() -> elevatorWrist.getAngleFromDistance(swerveDrive.getPose()))
.withTimeout(1.5)), CommandFactory.Auto.runIndexer(intake)),
.withTimeout(1.5)), CommandFactory.Auto.runIndexer(indexer)),
abort));
Command part2 = followPath2.deadlineWith(CommandFactory.intakeNote(intake),
Command part2 = followPath2.deadlineWith(CommandFactory.intakeNote(intake, indexer),
elevatorWrist.homePosition().withTimeout(1.0))
.andThen(Commands.either(Commands.none(),
Commands.sequence(CommandFactory.intakeNote(intake).alongWith(elevatorWrist
Commands.sequence(CommandFactory.intakeNote(intake, indexer).alongWith(elevatorWrist
.followPosition(() -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT,
() -> elevatorWrist.getAngleFromDistance(swerveDrive.getPose()))
.withTimeout(1.5)), CommandFactory.Auto.runIndexer(intake)),
.withTimeout(1.5)), CommandFactory.Auto.runIndexer(indexer)),
abort));
Command part3 = followPath3.alongWith(CommandFactory.intakeNote(intake))
Command part3 = followPath3.alongWith(CommandFactory.intakeNote(intake, indexer))
.andThen(
elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT,
Rotation2d.fromDegrees(32.5)).withTimeout(1.5))
.andThen(CommandFactory.Auto.runIndexer(intake))
.andThen(CommandFactory.Auto.runIndexer(indexer))
.andThen(elevatorWrist.homePosition().withTimeout(.5));


Command part1_dump = followPath1_dump.deadlineWith(CommandFactory.intakeNote(intake))
.andThen(Commands.either(Commands.none(),
Commands.sequence(CommandFactory.intakeNote(intake),
CommandFactory.Auto.runIndexer(intake)),
abort))
.andThen(elevatorWrist.homePosition().withTimeout(.1));
Command part2_dump = followPath2_dump.deadlineWith(CommandFactory.intakeNote(intake))
.andThen(Commands.either(Commands.none(),
Commands.sequence(CommandFactory.intakeNote(intake),
CommandFactory.Auto.runIndexer(intake)),
abort))
.andThen(elevatorWrist.homePosition().withTimeout(.1));
Command part3_dump = followPath3_dump.deadlineWith(CommandFactory.intakeNote(intake))
.andThen(Commands.either(Commands.none(),
Commands.sequence(CommandFactory.intakeNote(intake),
CommandFactory.Auto.runIndexer(intake)),
abort))
.andThen(elevatorWrist.homePosition().withTimeout(.1));
Command part4_dump = followPath4_dump.alongWith(CommandFactory.intakeNote(intake));
Command part1_dump =
followPath1_dump.deadlineWith(CommandFactory.intakeNote(intake, indexer))
.andThen(Commands.either(Commands.none(),
Commands.sequence(CommandFactory.intakeNote(intake, indexer),
CommandFactory.Auto.runIndexer(indexer)),
abort))
.andThen(elevatorWrist.homePosition().withTimeout(.1));
Command part2_dump =
followPath2_dump.deadlineWith(CommandFactory.intakeNote(intake, indexer))
.andThen(Commands.either(Commands.none(),
Commands.sequence(CommandFactory.intakeNote(intake, indexer),
CommandFactory.Auto.runIndexer(indexer)),
abort))
.andThen(elevatorWrist.homePosition().withTimeout(.1));
Command part3_dump =
followPath3_dump.deadlineWith(CommandFactory.intakeNote(intake, indexer))
.andThen(Commands.either(Commands.none(),
Commands.sequence(CommandFactory.intakeNote(intake, indexer),
CommandFactory.Auto.runIndexer(indexer)),
abort))
.andThen(elevatorWrist.homePosition().withTimeout(.1));
Command part4_dump = followPath4_dump.alongWith(CommandFactory.intakeNote(intake, indexer));

Command wait = Commands.waitSeconds(.01);
Command resetPosition = Commands.runOnce(() -> {
Expand Down
0 src/main/java/frc/robot/autos/Resnick3.java → ...in/java/frc/robot/autos/Resnick3.java.txt
View file
File renamed without changes.
0 src/main/java/frc/robot/autos/Resnick4.java → ...in/java/frc/robot/autos/Resnick4.java.txt
View file
File renamed without changes.
0 src/main/java/frc/robot/autos/Resnick5.java → ...in/java/frc/robot/autos/Resnick5.java.txt
View file
File renamed without changes.

0 comments on commit b595aa6

Please sign in to comment.