feat: made indexer probably interact better with other subsystems

flamingchickens1540 · Feb 5, 2024 · 5bd8339 · 5bd8339
1 parent 3fd56d7
commit 5bd8339
Show file tree

Hide file tree

Showing 9 changed files with 119 additions and 23 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -78,7 +78,7 @@ public static class Indexer {
         public static final double INTAKE_MOI = 0.025;
         public static final double FEEDER_MOI = 0.025;
         public static final int BEAM_BREAK_ID = 0;
-        public static final double VELOCITY_ERR_TOLERANCE_RPM = 10;
+        public static final int VELOCITY_ERR_TOLERANCE_RPM = 10;
 
 
     }

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -69,7 +69,7 @@ public class RobotContainer {
     // TODO: testing dashboard inputs, remove for comp
 
     /**
-     * The container for the robot. Contains subsystems, OI devices, and commands.
+     * The container for the robot. Contains subsystems, IO devices, and commands.
      */
     public RobotContainer() {
         switch (Constants.currentMode) {
@@ -173,8 +173,8 @@ public RobotContainer() {
     private void configureButtonBindings() {
         drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver));
         elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot));
-        copilot.rightBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.TOP));
-        copilot.leftBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM));
+        indexer.setDefaultCommand(new IntakeCommand(indexer, tramp));
+
         driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain));
         driver.b().onTrue(
                 Commands.runOnce(
@@ -183,10 +183,10 @@ private void configureButtonBindings() {
                 ).ignoringDisable(true)
         );
 
+        copilot.rightBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.TOP));
+        copilot.leftBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM));
         copilot.a().onTrue(new ShootSequence(shooter, indexer))
                 .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));
-
-        driver.a().onTrue(new IntakeCommand(indexer));
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java
@@ -2,29 +2,34 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.tramp.Tramp;
 
 public class IntakeCommand extends Command {
-
     private final Indexer indexer;
+    private final Tramp tramp;
 
-    public IntakeCommand(Indexer indexer) {
+    public IntakeCommand(Indexer indexer, Tramp tramp) {
         this.indexer = indexer;
+        this.tramp = tramp;
         addRequirements(indexer);
     }
 
+    // It should not be necessary to deal with the case of the note in the robot and not being detected,
+    // because the indexer will always be being used when the note is in transition (?)
+    // but maybe just for code safety this is good
+    // also it is hard to imagine that being a problem
+    // @zach
     @Override
-    public void initialize() {
-        indexer.setIntakePercent(0.4);
-    }
-
-
-    @Override
-    public boolean isFinished() {
-        return indexer.isNoteStaged();
+    public void execute() {
+        if (indexer.isNoteStaged() || tramp.isNoteStaged()) {
+            indexer.stopIntake();
+        } else {
+            indexer.setIntakePercent(0.5);
+        }
     }
 
     @Override
     public void end(boolean interrupted) {
-        indexer.setIntakePercent(0);
+        indexer.stopIntake();
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForShooter.java b/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForShooter.java
@@ -0,0 +1,25 @@
+package org.team1540.robot2024.commands.indexer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+
+public class PrepareIndexerForShooter extends Command {
+
+    private final Indexer indexer;
+
+    public PrepareIndexerForShooter(Indexer indexer) {
+        this.indexer = indexer;
+        addRequirements(indexer);
+    }
+
+    @Override
+    public void initialize() {
+        indexer.setFeederVelocity(1200);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return indexer.isFeederAtSetpoint();
+    }
+
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForTramp.java b/src/main/java/org/team1540/robot2024/commands/indexer/PrepareIndexerForTramp.java
@@ -0,0 +1,26 @@
+package org.team1540.robot2024.commands.indexer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+
+public class PrepareIndexerForTramp extends Command {
+
+    private final Indexer indexer;
+
+    // I'm not sure if the tramp should be done the same as the shooter here, but I think consistency wins?
+    public PrepareIndexerForTramp(Indexer indexer) {
+        this.indexer = indexer;
+        addRequirements(indexer);
+    }
+
+    @Override
+    public void initialize() {
+        indexer.setFeederVelocity(-600);
+    }
+
+    @Override
+    public boolean isFinished() {
+        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
@@ -17,9 +17,17 @@ public StageTrampCommand(Tramp tramp, Indexer indexer) {
 
     @Override
     public void initialize() {
-        tramp.setPercent(0.5);
         indexer.setFeederVelocity(-600);
-        indexer.setIntakePercent(0.5);
+    }
+
+    // This could instead be written like ShootSequence and PrepareIndexerForShooter, but perhaps that
+    // is more intelligent than what is needed for the tramp
+    @Override
+    public void execute() {
+        if (indexer.isFeederAtSetpoint()) {
+            tramp.setPercent(0.5);
+            indexer.setIntakePercent(0.5);
+        }
     }
 
     @Override
@@ -30,6 +38,7 @@ public boolean isFinished() {
     @Override
     public void end(boolean interrupted) {
         indexer.stopFeeder();
+        indexer.stopIntake();
         tramp.stop();
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java
@@ -1,16 +1,16 @@
 package org.team1540.robot2024.commands.shooter;
 
-import edu.wpi.first.wpilibj2.command.Command;
 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.subsystems.indexer.Indexer;
 import org.team1540.robot2024.subsystems.shooter.Shooter;
 
 public class ShootSequence extends SequentialCommandGroup {
     public ShootSequence(Shooter shooter, Indexer indexer) {
         addCommands(
                 Commands.parallel(
-                        indexer.feedToShooter(),
+                        new PrepareIndexerForShooter(indexer),
                         new PrepareShooterCommand(shooter)
                 ),
                 Commands.runOnce(() -> indexer.setIntakePercent(1), indexer)

diff --git a/src/main/java/org/team1540/robot2024/commands/tramp/StageTrampCommand.java b/src/main/java/org/team1540/robot2024/commands/tramp/StageTrampCommand.java
@@ -0,0 +1,26 @@
+package org.team1540.robot2024.commands.tramp;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import org.team1540.robot2024.commands.indexer.PrepareIndexerForTramp;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.tramp.Tramp;
+
+public class StageTrampCommand extends SequentialCommandGroup {
+    public StageTrampCommand(Tramp tramp, Indexer indexer) {
+        addCommands(
+                new PrepareIndexerForTramp(indexer),
+                Commands.parallel(
+                        Commands.runOnce(() -> indexer.setIntakePercent(0.5), indexer),
+                        Commands.runOnce(() -> tramp.setPercent(0.5), tramp),
+                        Commands.waitUntil(tramp::isNoteStaged)
+                ),
+                Commands.parallel(
+                        Commands.runOnce(indexer::stopAll),
+                        Commands.runOnce(tramp::stop, tramp)
+                )
+
+        );
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -1,6 +1,5 @@
 package org.team1540.robot2024.subsystems.indexer;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -59,9 +58,15 @@ public boolean isFeederAtSetpoint() {
     }
 
     public void stopFeeder() {
-        io.setFeederVoltage(0);
+        io.setFeederVoltage(0); // shouldn't stopFeeder probably set its velocity?
     }
     public void stopIntake() {
         io.setIntakeVoltage(0);
     }
+
+    public void stopAll() {
+        io.setIntakeVoltage(0);
+        io.setFeederVoltage(0);
+    }
+
 }