[Robot] Improved auto shooting reliability

SouthwestRoboticsProgramming · Aug 24, 2024 · a0ac8b5 · a0ac8b5
1 parent 3b5c3fd
commit a0ac8b5
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 11 deletions.
diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/IndexerFeedCommand.java b/Robot/src/main/java/com/swrobotics/robot/commands/IndexerFeedCommand.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 
+@Deprecated
 public final class IndexerFeedCommand extends Command {
     private final IndexerSubsystem indexer;
     private final Timer timer;

diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java b/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java
@@ -1,16 +1,12 @@
 package com.swrobotics.robot.commands;
 
-import java.util.ArrayList;
-import java.util.List;
-
 import com.swrobotics.robot.RobotContainer;
 import com.swrobotics.robot.config.NTData;
 import com.swrobotics.robot.subsystems.amp.AmpArm2Subsystem;
 import com.swrobotics.robot.subsystems.speaker.IntakeSubsystem.State;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 
 public final class RobotCommands {
     public static Command justShoot(RobotContainer robot) {
@@ -19,7 +15,7 @@ public static Command justShoot(RobotContainer robot) {
             Commands.waitUntil(robot.shooter::isReadyToShoot)
                 .withTimeout(NTData.SHOOTER_AUTO_READY_TIMEOUT.get()),
             Commands.waitSeconds(NTData.SHOOTER_AUTO_AFTER_READY_DELAY.get()),
-            new IndexerFeedCommand(robot.indexer)
+            robot.indexer.getFeedCommand()
         );
     }
 
@@ -57,7 +53,8 @@ public static Command aimAndShoot(RobotContainer robot, boolean waitForNote, boo
         //     cmd = cmd.finallyDo(() -> robot.drive.setEstimatorIgnoreVision(true));
         // }
 
-        cmd = cmd.andThen(new IndexerFeedCommand(robot.indexer));
+        // cmd = cmd.andThen(new IndexerFeedCommand(robot.indexer));
+        cmd = cmd.andThen(robot.indexer.getFeedCommand());
         return cmd;
     }
 
@@ -75,7 +72,7 @@ public static Command ejectHard(RobotContainer robot) {
             Commands.waitUntil(() -> robot.shooter.isReadyToShoot())
                     .withTimeout(NTData.SHOOTER_AUTO_READY_TIMEOUT.get()),
                 // Commands.waitSeconds(NTData.SHOOTER_AUTO_AFTER_READY_DELAY.get()),
-                new IndexerFeedCommand(robot.indexer)
+                robot.indexer.getFeedCommand()
         );
     }
 

diff --git a/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java b/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java
@@ -90,7 +90,6 @@ public static void updateFlywheels(double flywheelVelocity) {
     }
 
     public static void setShooting(boolean shooting) {
-        System.out.println("Shooting: " + shooting);
         if (shooting) {
             shooterPivot.setColor(new Color8Bit(Color.kBlueViolet));
             view.setBackgroundColor(new Color8Bit(Color.kYellow));

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java
@@ -8,9 +8,13 @@
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 
 public final class IndexerSubsystem extends SubsystemBase {
     private final VictorSP topMotor = new VictorSP(IOAllocation.RIO.PWM_INDEXER_TOP_MOTOR);
@@ -33,10 +37,15 @@ public IndexerSubsystem(IntakeSubsystem intake) {
     }
 
     public void setFeedToShooter(boolean feedToShooter) {
+        System.out.println("yah");
         this.feedToShooter = feedToShooter;
     }
 
     public boolean hasPiece() {
+        if (RobotBase.isSimulation()) {
+            return false;
+        }
+
         // Beam break is true when not blocked
         return !beamBreak.get();
 //        return !beamBreakDebounce.calculate(beamBreak.get());
@@ -174,4 +183,13 @@ public IndexerSubsystem setAutoReindexEnable(boolean autoReindexEnable) {
         this.autoReindexEnable = autoReindexEnable;
         return this;
     }
+
+    public Command getFeedCommand() {
+        return Commands.run(() -> setFeedToShooter(true))
+        .until(() -> !hasPiece())
+        .andThen(new WaitCommand(NTData.INDEXER_FEED_ADDITIONAL_TIME.get()))
+        .andThen(Commands.runOnce(() -> setFeedToShooter(false)))
+        .withTimeout(1.0); // Emergency timeout for if the note does not exit the robot
+
+    }
 }
diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java
@@ -171,6 +171,8 @@ public void periodic() {
         } else {
             activeMode.set("none");
         }
+
+        afterShootDelay.calculate(indexer.hasPiece());
     }
 
     private void handleSpeaker() {
@@ -180,7 +182,7 @@ private void handleSpeaker() {
         Translation2d robotVelocity = velocityTo(target);
 
         AimCalculator.Aim aim = TableAimCalculator.INSTANCE.calculateAim(distToSpeaker, robotVelocity.getX());
-        if (/* afterShootDelay.calculate(indexer.hasPiece()) && */(aim != null)) { // Fixme
+        if (afterShootDelay.calculate(indexer.hasPiece()) && (aim != null)) { // Fixme
             pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU);
             flywheel.setTargetVelocity(aim.flywheelVelocity());
             targetAim = aim;
@@ -302,7 +304,6 @@ private Rotation2d getAimCorrection(Translation2d target, Supplier<Double> flyti
         Translation2d robotVelocity = velocityTo(target);
         double missAmount = flytime.get() * robotVelocity.getY();
 
-        System.out.println(flytime.get());
         double correctionRad = -Math.atan2(missAmount, distanceTo(target)) + Math.toRadians(otherCorrection.get());
         return new Rotation2d(correctionRad);
     }
@@ -320,7 +321,7 @@ public Command getSpeakerAimCommand() {
     }
 
     public Command getSpeakerSnapCommand() {
-        return Commands.defer(() -> getDynamicSnapCommand(getSpeakerPosition(), () -> distanceTo(getSpeakerPosition()) / 10.0, () -> 0.0), Set.of());
+        return Commands.defer(() -> getDynamicSnapCommand(getSpeakerPosition(), () -> distanceTo(getSpeakerPosition()) / 10.0, () -> 0.0), Set.of()).alongWith(new PrintCommand("Shooting"));
     }
 
     public Command getLobAimCommand() {