From: Wesley28w Date: Tue, 21 Apr 2026 14:10:00 +0000 (-0700) Subject: Cleaned autos, made dynamic auto builder cleaner, implmeented command with stop X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=6009959db809c0303c8261b228447f8288fde79b;p=FRC2026.git Cleaned autos, made dynamic auto builder cleaner, implmeented command with stop --- diff --git a/src/main/deploy/pathplanner/autos/LeftSwipeConservative.auto b/src/main/deploy/pathplanner/autos/LeftSwipeConservative.auto new file mode 100644 index 0000000..6315c95 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftSwipeConservative.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftConservativeSwipe1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto deleted file mode 100644 index 440a1ea..0000000 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ /dev/null @@ -1,12 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New New Auto.auto b/src/main/deploy/pathplanner/autos/New New Auto.auto deleted file mode 100644 index 440a1ea..0000000 --- a/src/main/deploy/pathplanner/autos/New New Auto.auto +++ /dev/null @@ -1,12 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New New New Auto.auto b/src/main/deploy/pathplanner/autos/New New New Auto.auto deleted file mode 100644 index 440a1ea..0000000 --- a/src/main/deploy/pathplanner/autos/New New New Auto.auto +++ /dev/null @@ -1,12 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New New New New Auto.auto b/src/main/deploy/pathplanner/autos/New New New New Auto.auto deleted file mode 100644 index 440a1ea..0000000 --- a/src/main/deploy/pathplanner/autos/New New New New Auto.auto +++ /dev/null @@ -1,12 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New New New New New Auto.auto b/src/main/deploy/pathplanner/autos/New New New New New Auto.auto deleted file mode 100644 index 440a1ea..0000000 --- a/src/main/deploy/pathplanner/autos/New New New New New Auto.auto +++ /dev/null @@ -1,12 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RightSwipeConservative.auto b/src/main/deploy/pathplanner/autos/RightSwipeConservative.auto new file mode 100644 index 0000000..169ee6a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RightSwipeConservative.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RightConservativeSwipe1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fcda69f..dfb7fd5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -296,8 +296,17 @@ public class RobotContainer { DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake, breaker); - String leftDynamicDoubleSwipe = "LeftDynamicDoubleLiberalSwipe"; - addAuto(leftDynamicDoubleSwipe, dynamicAutoBuilder.getLeftDynamicDoubleLiberalSwipe()); + // names + String leftDynamicLiberalDoubleSwipe = "LeftDynamicDoubleLiberalSwipe"; + String rightDynamicLiberalDoubleSwipe = "RightDynamicDoubleLiberalSwipe"; + String leftDynamicConservativeDoubleSwipe = "LeftDynamicDoubleLiberalSwipe"; + String rightDynamicConservativeDoubleSwipe = "RightDynamicDoubleLiberalSwipe"; + + // add commands + addAuto(leftDynamicLiberalDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleLiberalSwipe(true)); + addAuto(rightDynamicLiberalDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleLiberalSwipe(false)); + addAuto(leftDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleLiberalSwipe(true)); + addAuto(rightDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleLiberalSwipe(false)); // put the Chooser on the SmartDashboard SmartDashboard.putData("Auto chooser", autoChooser); diff --git a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java index 98426b0..1bbd748 100644 --- a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java +++ b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.*; import frc.robot.commands.gpm.IntakeCommand; import frc.robot.commands.gpm.RunSpindexer; +import frc.robot.commands.gpm.RunSpindexerWithStop; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.PowerControl.BreakerConstants; import frc.robot.subsystems.PowerControl.EMABreaker; @@ -19,62 +20,98 @@ public class DynamicAutoBuilder { private final Turret turret; private final Hood hood; private final Intake intake; - private final EMABreaker breaker; - public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake, EMABreaker breaker) { + public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { this.spindexer = spindexer; this.turret = turret; this.hood = hood; this.intake = intake; - this.breaker = breaker; } - public Command getLeftDynamicDoubleLiberalSwipe() { - return new SequentialCommandGroup( - new InstantCommand(() -> { - hood.forceHoodDown(true); - intake.extend(); - intake.spinStart(); - }), + /* + * Autos have no named commands within them. They must be added here + * Still need to make one method to call that four command block in each sequential + */ - new PathPlannerAuto("LeftSwipeOne"), + public Command getDynamicDoubleLiberalSwipe(boolean left) { + return new SequentialCommandGroup( + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeOne" : "RightSwipeOne"), + startShootingCommand(), + runSpindexerWithAbort(), - new InstantCommand(() -> hood.forceHoodDown(false)), + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), + startShootingCommand(), runSpindexerWithAbort(), - new InstantCommand(() -> hood.forceHoodDown(true)), - new PathPlannerAuto("LeftSwipeTwo"), - new InstantCommand(() -> hood.forceHoodDown(false)), + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), + startShootingCommand(), + runSpindexerWithAbort(), + + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), + startShootingCommand(), + runSpindexerWithAbort() + ); + } + + public Command getDynamicDoubleConservativeSwipe(boolean left) { + return new SequentialCommandGroup( + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeConservative" : "RightSwipeConservative"), + startShootingCommand(), runSpindexerWithAbort(), - new InstantCommand(() -> hood.forceHoodDown(true)), - new PathPlannerAuto("LeftSwipeTwo"), - new InstantCommand(() -> hood.forceHoodDown(false)), + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), + startShootingCommand(), runSpindexerWithAbort(), - new InstantCommand(() -> hood.forceHoodDown(true)), - new PathPlannerAuto("LeftSwipeTwo"), - new InstantCommand(() -> hood.forceHoodDown(false)), + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), + startShootingCommand(), runSpindexerWithAbort(), - new InstantCommand(() -> hood.forceHoodDown(true)) - ); + + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), + startShootingCommand(), + runSpindexerWithAbort() + ); } - public Command getRightDynamicDoubleLiberalSwipe() { + public Command getDynamicDoubleShallowSwipe(boolean left) { return new SequentialCommandGroup( - new InstantCommand(() -> hood.forceHoodDown(true)), - - new PathPlannerAuto("RightSwipeOne"), + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeThree" : "RightSwipeThree"), + startShootingCommand(), runSpindexerWithAbort(), - new PathPlannerAuto("RightSwipeTwo"), + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), + startShootingCommand(), runSpindexerWithAbort(), - new PathPlannerAuto("RightSwipeTwo"), + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), + startShootingCommand(), runSpindexerWithAbort(), + + departCommand(), + new PathPlannerAuto(left ? "LeftSwipeTwo" : "RightSwipeTwo"), + startShootingCommand(), + runSpindexerWithAbort() + ); + } - new PathPlannerAuto("RightSwipeTwo"), - runSpindexerWithAbort()); + private Command departCommand() { + return new InstantCommand(() -> { + hood.forceHoodDown(true); + intake.extend(); + intake.spinStart(); + spindexer.stopSpindexer(); + }); } private Command runSpindexerWithAbort() { @@ -83,18 +120,26 @@ public class DynamicAutoBuilder { // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new // WaitCommand(5.0)); - var timer = new Timer(); - timer.start(); + // var timer = new Timer(); + // timer.start(); // return new RunSpindexer(spindexer, turret, hood, intake).raceWith( // new WaitUntilCommand(() -> spindexer.spinningAir() && timer.hasElapsed(3.0))); // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new WaitCommand(3.0)); - return new RunSpindexer(spindexer, turret, hood, intake).until(() -> spindexer.spinningAir() && timer.hasElapsed(3.0)); + // return new RunSpindexer(spindexer, turret, hood, intake).until(() -> spindexer.spinningAir() && timer.hasElapsed(3.0)); + // has an isFinnished so it should work + return new RunSpindexerWithStop(spindexer, turret, hood, intake); // return new ParallelDeadlineGroup(new WaitUntilCommand(() -> // spindexer.spinningAir()), new RunSpindexer(spindexer, turret, hood, intake)); // return new ParallelDeadlineGroup(new WaitCommand(5.0), new // RunSpindexer(spindexer, turret, hood, intake)); } + + private Command startShootingCommand() { + return new InstantCommand(() -> { + new InstantCommand(() -> hood.forceHoodDown(false)); + }); + } } diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java index 3dbbbf1..9bd1550 100644 --- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java +++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java @@ -30,7 +30,6 @@ public class RunSpindexer extends Command { private double storedIntakeSpeed = 0.0; - public RunSpindexer(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { this.spindexer = spindexer; this.turret = turret; diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java b/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java index 86b45d2..71c3f5e 100644 --- a/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java +++ b/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java @@ -2,12 +2,16 @@ package frc.robot.commands.gpm; import org.littletonrobotics.junction.Logger; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.constants.Constants; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.Intake.IntakeConstants; @@ -31,6 +35,9 @@ public class RunSpindexerWithStop extends Command { private double storedIntakeSpeed = 0.0; + + private Timer runTimer = new Timer(); + private boolean seizing; public RunSpindexerWithStop(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { this.spindexer = spindexer; @@ -49,8 +56,8 @@ public class RunSpindexerWithStop extends Command { @Override public void initialize() { wasHoodForcedDown = hood.getHoodForcedDown(); - timer.reset(); - timer.start(); + runTimer.reset(); + runTimer.start(); } @Override @@ -93,6 +100,16 @@ public class RunSpindexerWithStop extends Command { intake.spin(storedIntakeSpeed); } } + + // need to test + // // intake jostle + // if (runTimer.hasElapsed(3.0)) { + // seizing = true; + // new IntakeMovementCommand(intake).until(() -> !seizing); + // } else { + // seizing = false; + // } + if (!Constants.DISABLE_SMART_DASHBOARD) { SmartDashboard.putBoolean("Spindexer Jamming", reversing); } @@ -104,10 +121,8 @@ public class RunSpindexerWithStop extends Command { reversing = false; } - private Timer timer = new Timer(); - @Override public boolean isFinished() { - return spindexer.spinningAir() && timer.hasElapsed(1.0); + return spindexer.spinningAir() && runTimer.hasElapsed(1.0); } }