From 35a726be7cf7be8f01683151f111723bd20c2d38 Mon Sep 17 00:00:00 2001
From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com>
Date: Tue, 6 Feb 2024 14:55:59 -0800
Subject: [PATCH] fix: made feeder sequence have the intake running, and made
 the intake not run while feeder speeds up for shooting

---
 .../commands/indexer/OuttakeCommand.java      | 30 +++++++++++++++++++
 ...oter.java => PrepareFeederForShooter.java} |  8 ++---
 .../commands/indexer/StageTrampCommand.java   |  1 +
 .../commands/shooter/ShootSequence.java       |  8 +++--
 .../robot2024/subsystems/indexer/Indexer.java |  2 +-
 5 files changed, 41 insertions(+), 8 deletions(-)
 create mode 100644 src/main/java/org/team1540/robot2024/commands/indexer/OuttakeCommand.java
 rename src/main/java/org/team1540/robot2024/commands/indexer/{PrepareIndexerForShooter.java => PrepareFeederForShooter.java} (65%)

diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/OuttakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/OuttakeCommand.java
new file mode 100644
index 00000000..0091d1b3
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/indexer/OuttakeCommand.java
@@ -0,0 +1,30 @@
+package org.team1540.robot2024.commands.indexer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+
+public class OuttakeCommand extends Command {
+
+    private final Indexer indexer;
+
+    public OuttakeCommand(Indexer indexer) {
+        this.indexer = indexer;
+        addRequirements(indexer);
+    }
+
+    @Override
+    public void initialize() {
+        indexer.setIntakePercent(-0.5);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return !indexer.isNoteStaged();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexer.setIntakePercent(0);
+    }
+
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForShooter.java b/src/main/java/org/team1540/robot2024/commands/indexer/PrepareFeederForShooter.java
similarity index 65%
rename from src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForShooter.java
rename to src/main/java/org/team1540/robot2024/commands/indexer/PrepareFeederForShooter.java
index 7e8f8b31..ba3d9519 100644
--- a/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForShooter.java
+++ b/src/main/java/org/team1540/robot2024/commands/indexer/PrepareFeederForShooter.java
@@ -3,11 +3,11 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import org.team1540.robot2024.subsystems.indexer.Indexer;
 
-public class PrepareIndexerForShooter extends Command {
+public class PrepareFeederForShooter extends Command {
 
     private final Indexer indexer;
 
-    public PrepareIndexerForShooter(Indexer indexer) {
+    public PrepareFeederForShooter(Indexer indexer) {
         this.indexer = indexer;
         addRequirements(indexer);
     }
@@ -15,12 +15,12 @@ public PrepareIndexerForShooter(Indexer indexer) {
     @Override
     public void initialize() {
         indexer.setFeederVelocity(1200);
-        indexer.setIntakePercent(-0.5);
+
     }
 
     @Override
     public boolean isFinished() {
-        return indexer.isFeederAtSetpoint() && !indexer.isNoteStaged();
+        return indexer.isFeederAtSetpoint();
     }
 
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java
index 6bf74ce5..ffb74474 100644
--- a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java
+++ b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java
@@ -18,6 +18,7 @@ public StageTrampCommand(Tramp tramp, Indexer indexer) {
     public void initialize() {
         tramp.setPercent(0.5);
         indexer.setFeederVelocity(-600);
+        indexer.setIntakePercent(0.5);
     }
 
     @Override
diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java
index 39762692..aba7021d 100644
--- a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java
+++ b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java
@@ -2,7 +2,8 @@
 
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import org.team1540.robot2024.commands.indexer.PrepareIndexerForShooter;
+import org.team1540.robot2024.commands.indexer.OuttakeCommand;
+import org.team1540.robot2024.commands.indexer.PrepareFeederForShooter;
 import org.team1540.robot2024.subsystems.indexer.Indexer;
 import org.team1540.robot2024.subsystems.shooter.Shooter;
 
@@ -10,8 +11,9 @@ public class ShootSequence extends SequentialCommandGroup {
     public ShootSequence(Shooter shooter, Indexer indexer) {
         addCommands(
                 Commands.parallel(
-                        new PrepareIndexerForShooter(indexer),
-                        new PrepareShooterCommand(shooter)
+                        new PrepareFeederForShooter(indexer),
+                        new PrepareShooterCommand(shooter),
+                        new OuttakeCommand(indexer)
                 ),
                 Commands.runOnce(() -> indexer.setIntakePercent(1), indexer)
                 // TODO: Add a wait for having completed the shot (steady then current spike/velocity dip and then back down?)
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
index 273f06ab..1b326c71 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -58,7 +58,7 @@ public boolean isFeederAtSetpoint() {
     }
 
     public void stopFeeder() {
-        io.setFeederVoltage(0); // shouldn't stopFeeder probably set its velocity?
+        io.setFeederVoltage(0);
     }
     public void stopIntake() {
         io.setIntakeVoltage(0);