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 (file)
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 (file)
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 (file)
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 (file)
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 (file)
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 (file)
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 (file)
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 fcda69fcacd200c4d361fdeb19827704e71b1e81..dfb7fd540bae2e9c67b45ebfeb766e30b9e7371e 100644 (file)
--- 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 98426b04911dba23b3df141b6c37bbe81d9ed144..1bbd748cd3ecab330d4d2424ddb5b40d7cec34ee 100644 (file)
--- 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 3dbbbf1ec72469db9fb1f17374412c2094440e92..9bd1550c867169e718f8d7e7cc9f29a923ae98b5 100644 (file)
--- 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 86b45d26dd50b84880c7d22e26f3434bb75a3e28..71c3f5e7de57e5e7b0d8c1fede00a9ad9299cd1a 100644 (file)
--- 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);
     }
 }