From f61a483cdc1da782229c44d4538766d2250f5283 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Thu, 3 Oct 2024 13:57:55 -0400 Subject: [PATCH] one piece mobilities --- .../paths/Blue Amp Side Mobility.path | 8 +++---- .../paths/Red Amp Side Mobility.path | 8 +++---- .../com/stuypulse/robot/RobotContainer.java | 21 +++++++++++++----- .../auton/CenterMobilityWithWait.java | 22 +++++++++++++++++++ .../auton/SideAutons/OnePieceAmpSide.java | 6 ++--- 5 files changed, 48 insertions(+), 17 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/CenterMobilityWithWait.java diff --git a/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path index 9e7c37c..9aff4c0 100644 --- a/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path +++ b/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.9026396514331507, - "y": 6.9956624528633675 + "x": 2.496814123442796, + "y": 7.4598028796628215 }, "prevControl": { - "x": 1.9026396514331507, - "y": 6.9956624528633675 + "x": 1.496814123442796, + "y": 7.4598028796628215 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path index 45101df..9a5d83d 100644 --- a/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path +++ b/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.9026396514331507, - "y": 6.9956624528633675 + "x": 2.4851032868970147, + "y": 7.436381206571257 }, "prevControl": { - "x": 1.9026396514331507, - "y": 6.9956624528633675 + "x": 1.4851032868970147, + "y": 7.436381206571257 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e876fac..ddbcd26 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,6 +13,7 @@ import com.stuypulse.robot.commands.arm.ArmToSpeaker; import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.auton.CenterMobilityWithWait; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; @@ -276,6 +277,11 @@ public void configureAutons() { AutonConfig MOBILITY_BLUE = new AutonConfig("Mobility", Mobility::new, "Mobility"); AutonConfig MOBILITY_RED = new AutonConfig("Mobility", Mobility::new, "Mobility"); + AutonConfig CENTER_MOBILITY_BLUE = new AutonConfig("Center Mobility", CenterMobilityWithWait::new, + "Mobility"); + AutonConfig CENTER_MOBILITY_RED = new AutonConfig("Center Mobility", CenterMobilityWithWait::new, + "Mobility"); + // BCA AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new, "Blue Center to B", "Blue B to Center", "Blue Center to C", "Blue C to Shoot Before A", "Blue Center to A", "Blue A to Center"); @@ -294,10 +300,10 @@ public void configureAutons() { AutonConfig ADEF_RED = new AutonConfig("5 ADEF", FivePieceADEF::new, "Red Amp to A", "Red A to D", "Red D to Shoot", "Red D Shoot to E", "Red E to Shoot", "Red E Shoot to F"); - // AutonConfig AMP_SIDE_ONE_PIECE_BLUE = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, - // "Blue Amp Side Mobility"); - // AutonConfig AMP_SIDE_ONE_PIECE_RED = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, - // "Red Amp Side Mobility"); + AutonConfig AMP_SIDE_ONE_PIECE_BLUE = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, + "Blue Amp Side Mobility"); + AutonConfig AMP_SIDE_ONE_PIECE_RED = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, + "Red Amp Side Mobility"); AutonConfig SOURCE_SIDE_ONE_PIECE_BLUE = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new, "Blue Source Side Mobility"); @@ -305,8 +311,11 @@ public void configureAutons() { AutonConfig SOURCE_SIDE_ONE_PIECE_RED = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new, "Red Source Side Mobility"); - // AMP_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser); - // AMP_SIDE_ONE_PIECE_RED.registerRed(autonChooser); + AMP_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser); + AMP_SIDE_ONE_PIECE_RED.registerRed(autonChooser); + + CENTER_MOBILITY_BLUE.registerBlue(autonChooser); + CENTER_MOBILITY_RED.registerRed(autonChooser); SOURCE_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser); SOURCE_SIDE_ONE_PIECE_RED.registerRed(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CenterMobilityWithWait.java b/src/main/java/com/stuypulse/robot/commands/auton/CenterMobilityWithWait.java new file mode 100644 index 0000000..a5e5132 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/CenterMobilityWithWait.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class CenterMobilityWithWait extends SequentialCommandGroup { + + public CenterMobilityWithWait(PathPlannerPath... paths) { + addCommands( + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + new WaitCommand(10), + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]) + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java index 6b9dceb..b78cc6f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java @@ -24,9 +24,9 @@ public OnePieceAmpSide(PathPlannerPath... paths) { new IntakeSetAcquire(), // Mobility - SwerveDrive.getInstance().followPathCommand(paths[0]), - new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).andThen(ShootRoutine.fromAnywhere()) - .onlyIf(() -> Intake.getInstance().hasNote() || Shooter.getInstance().hasNote()) + SwerveDrive.getInstance().followPathCommand(paths[0]) + // new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).andThen(ShootRoutine.fromAnywhere()) + // .onlyIf(() -> Intake.getInstance().hasNote() || Shooter.getInstance().hasNote()) ); }