fix intake auto commands + path markers

diff --git a/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path b/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path
index a4dd615cfc012e62089b7b5f8ba39ac4c7d3f759..1936de6e81a667c6924b47f4dc00fef563da46c7 100644 (file)
--- a/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path
+++ b/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path
@@ -90,6 +90,12 @@
               "data": {
                 "name": "Stop Intake Rollers"
               }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "Retract Intake"
+              }
             }
           ]
         }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f02e54b5155d83cbe7c251aa8424701dbdbf577e..97f3fe35c5989aaa6fa7b7b0ecc5ce39c73bc76f 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 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 edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.commands.DoNothing;
@@ -198,19 +199,11 @@ public class RobotContainer {
     if (intake != null){
       NamedCommands.registerCommand("Extend Intake", new InstantCommand(()-> intake.extend()));
       NamedCommands.registerCommand("Retract Intake", new InstantCommand(()-> intake.retract()));
-    }
-
-    if (intake != null && spindexer != null){ 
-      NamedCommands.registerCommand("Spin Intake Rollers", new ParallelCommandGroup(
-        new InstantCommand(()->intake.spin(IntakeConstants.SPEED))
-      ));
-      NamedCommands.registerCommand("Stop Intake Rollers", new ParallelCommandGroup(
-        new InstantCommand(()->intake.spinStop())
-      ));
-      Command intakeMovement = new IntakeMovementCommand(intake);
-      NamedCommands.registerCommand("Start Intake Seizure", new InstantCommand(()-> intakeMovement.schedule()));
-      NamedCommands.registerCommand("Stop Intake Seizure", new InstantCommand(()-> intakeMovement.cancel()));
-
+      NamedCommands.registerCommand("Intermediate Extend", new InstantCommand(()-> intake.intermediateExtend()));
+      NamedCommands.registerCommand("Spin Intake Rollers", new InstantCommand(()-> intake.spinStart()));
+      NamedCommands.registerCommand("Stop Intake Rollers", new InstantCommand(()-> intake.spinStop()));
+      NamedCommands.registerCommand("Start Intake Seizure", new InstantCommand(()-> new IntakeMovementCommand(intake).schedule()));
+      NamedCommands.registerCommand("Stop Intake Seizure", new InstantCommand(()-> CommandScheduler.getInstance().cancelAll()));
     }
 
     if (turret != null && drive != null && hood != null && shooter != null && spindexer != null){