Spin other subsystems with a delay after spinning the shooter

src/main/java/frc/robot/RobotContainer.java

@@ -39,6 +39,7 @@
 import frc.robot.commands.hopper.SpinHopper;
 import frc.robot.commands.intake.SpinIntake;
 import frc.robot.commands.intake.StopIntake;
+import frc.robot.commands.intakeDeployment.RunDeployer;
 import frc.robot.commands.intakeDeployment.SetDeploymentState;
 import frc.robot.commands.intakeDeployment.ToggleDeployment;
 import frc.robot.commands.parallels.RunHopperAndFeeder;
@@ -152,7 +153,7 @@ public RobotContainer() {
                                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(
                                                 new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null;
                             apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null;
-                            controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
+                            controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase,intakeDeployer, this) : null;
                                  lightStripSubsystem = new LightStripSubsystem(drivebase, shootState);
             }
                         case REPLAY -> {
@@ -169,7 +170,7 @@ public RobotContainer() {
                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
                 apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null;
-                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
+                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this) : null;
                                 lightStripSubsystem = new LightStripSubsystem(drivebase, shootState);
 
             }
@@ -187,7 +188,7 @@ public RobotContainer() {
 
                                                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
-                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
+                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this) : null;
                 apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null;
                                 lightStripSubsystem = new LightStripSubsystem(drivebase, shootState);
 
@@ -282,7 +283,7 @@ private void setUpAutoFactory() {
         }
 
         private void configureBindings() {
-                controller.a().onTrue(new ToggleDeployment(intakeDeployer));
+                controller.a().onTrue(new ToggleDeployment(intakeDeployer, controllerSubsystem));
                 controller.b().onTrue(new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED));
                 controller.y().onTrue(new ClimberUp(climberSubsystem));
                 controller.x().onTrue(new ClimberDown(climberSubsystem));
@@ -293,9 +294,7 @@ private void configureBindings() {
                 controller.leftTrigger().onTrue((new ResetAll(anglerSubsystem, climberSubsystem, 
                         intakeDeployer, intakeSubsystem, shooterSubsystem, turretSubsystem, shootState)));
                 driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED)));
-                if (controllerSubsystem != null) {
-                        steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem));
-                }
+
 
                 // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
                 // new Trigger(m_exampleSubsystem::exampleCondition)
@@ -308,6 +307,7 @@ private void configureBindings() {
 
                 intakeSubsystem.setDefaultCommand(new SpinIntake(intakeSubsystem, intakeDeployer));
             if (controllerSubsystem != null) {
+                intakeDeployer.setDefaultCommand(new RunDeployer(intakeDeployer));
                 anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem));
                 shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem));
                 turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem));
@@ -457,7 +457,7 @@ public void putShuffleboardCommands() {
 
                         SmartDashboard.putData(
                                         "intakedeployer/InitlizeDeployer",
-                                        new ToggleDeployment(intakeDeployer));
+                                        new ToggleDeployment(intakeDeployer, controllerSubsystem));
                         SmartDashboard.putData(
                                         "Spin Intake",
                                         new SpinIntake(intakeSubsystem, intakeDeployer));

src/main/java/frc/robot/commands/intakeDeployment/RunDeployer.java

@@ -5,6 +5,7 @@
 package frc.robot.commands.intakeDeployment;
 
 import frc.robot.constants.Constants;
+import frc.robot.subsystems.ControllerSubsystem;
 import frc.robot.subsystems.IntakeDeployerSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 
@@ -22,11 +23,11 @@ public void initialize() {
 
   @Override
   public void execute() {
-    switch (subsystem.getDeploymentState()) {
-      case UP -> subsystem.setSpeed(Constants.INTAKE_RETRACTION_SPEED);
-      case DOWN -> subsystem.setSpeed(Constants.INTAKE_DEPLOYER_SPEED);
-      case STOPPED -> subsystem.stopMotors();
-      default -> subsystem.stopMotors();
+      switch (subsystem.getDeploymentState()) {
+        case UP -> subsystem.setSpeed(Constants.INTAKE_RETRACTION_SPEED);
+        case DOWN -> subsystem.setSpeed(Constants.INTAKE_DEPLOYER_SPEED);
+        case STOPPED -> subsystem.stopMotors();
+        default -> subsystem.stopMotors();
     }
   }
 

src/main/java/frc/robot/commands/intakeDeployment/ToggleDeployment.java

@@ -7,23 +7,26 @@
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.constants.Constants;
 import frc.robot.constants.enums.DeploymentState;
+import frc.robot.subsystems.ControllerSubsystem;
 import frc.robot.subsystems.IntakeDeployerSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 
 public class ToggleDeployment extends LoggableCommand {
   private final IntakeDeployerSubsystem subsystem;
   private final Timer timer;
+  private final ControllerSubsystem controller;
 
-  public ToggleDeployment(IntakeDeployerSubsystem subsystem) {
+  public ToggleDeployment(IntakeDeployerSubsystem subsystem, ControllerSubsystem controller) {
     timer = new Timer();
     this.subsystem = subsystem;
+    this.controller = controller;
     addRequirements(subsystem);
   }
 
   @Override
-  public void initialize() {
-    subsystem.toggleState();
-    timer.restart();
+  public void initialize() { 
+      subsystem.toggleState(controller.canIntakeDeploy());
+      timer.restart();
   }
 
   @Override
@@ -42,7 +45,9 @@ public void end(boolean interrupted) {
 
   @Override
   public boolean isFinished() {
-    boolean limitSwitch = subsystem.getDeploymentState() == DeploymentState.UP ? subsystem.getFwdLimitSwitchState() : subsystem.getRevLimitSwitchState();
-    return timer.hasElapsed(Constants.INTAKE_DEPLOYER_BURNOUT_TIMER) || limitSwitch || subsystem.getDeploymentState() == DeploymentState.STOPPED;
+    boolean limitSwitch = subsystem.getDeploymentState() == DeploymentState.UP ? subsystem.getFwdLimitSwitchState()
+        : subsystem.getRevLimitSwitchState();
+    return timer.hasElapsed(Constants.INTAKE_DEPLOYER_BURNOUT_TIMER) || limitSwitch
+        || subsystem.getDeploymentState() == DeploymentState.STOPPED;
   }
 }

src/main/java/frc/robot/constants/GameConstants.java

@@ -36,7 +36,7 @@ public enum Mode {
     public static final boolean ENABLE_LOGGING = true;
 
     //Debugs
-    public static final boolean DEBUG = false;
+    public static final boolean DEBUG = true;
     public static final boolean ARM_DEBUG = true;
 
     //Joystick
@@ -122,8 +122,8 @@ public enum Mode {
     public static final double TURRET_HOME_ANGLE = 0.0; //Turret facing forward
     public static final double TURRET_MIN_ANGLE = -92;
     public static final double TURRET_MAX_ANGLE = 92;
-    public static final double TURRET_LIMIT_SPEED = 0.2;
-    public static final double TURRET_PID_DISTANCE_THRESHOLD = 45; /* TODO: Change later
+    public static final double TURRET_LIMIT_SPEED = 0.1;
+    public static final double TURRET_PID_DISTANCE_THRESHOLD = 5; /* TODO: Change later
     Minimum target encoder distance needed to use the longer pid slot*/
 
 

src/main/java/frc/robot/subsystems/ControllerSubsystem.java

@@ -12,19 +12,23 @@
 import org.littletonrobotics.junction.Logger;
 
 import frc.robot.RobotContainer;
+import frc.robot.commands.intakeDeployment.ToggleDeployment;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.Constants;
+import frc.robot.constants.enums.DeploymentState;
 import frc.robot.constants.enums.ShootingState.ShootState;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 
 public class ControllerSubsystem extends SubsystemBase {
 
     private static final double STOP_DELAY_SECONDS = 0.5;
+    private static final double SHOOT_DELAY_SECONDS = 0.5;
 
     // Placeholder target poses until real field target values are finalized
     private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION,
@@ -47,10 +51,10 @@ public class ControllerSubsystem extends SubsystemBase {
 
     // Placeholder fixed-state settings.
     private static final ShotTargets STOPPED_TARGETS = new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false,
-            false);
+            false,false);
     //3.25 meters away
-    private static final ShotTargets FIXED_TARGETS = new ShotTargets(21.16, -2945.21, 0, 3.25, true, true);
-    private static final ShotTargets FIXED_2_TARGETS = new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true);
+    private static final ShotTargets FIXED_TARGETS = new ShotTargets(21.16, -2945.21, 0, 3.25, true, true,true);
+    private static final ShotTargets FIXED_2_TARGETS = new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true,true);
 
     // Placeholder pose-driven profiles.
     private static final PoseControlProfile BLUE_HUB_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 32.0, 230.0,
@@ -63,18 +67,21 @@ public class ControllerSubsystem extends SubsystemBase {
             -14.0);
 
     private final SwerveSubsystem drivebase;
+    private final IntakeDeployerSubsystem intakeDeployer;
     private final RobotContainer robotContainer;
     private final Timer stopDelayTimer = new Timer();
+    private final Timer shootDelayTimer = new Timer();
 
     private ShootState previousState;
     private ShotTargets activeTargets;
     private boolean driverActivatedShooting = false;
 
-    public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer) {
+    public ControllerSubsystem(SwerveSubsystem drivebase, IntakeDeployerSubsystem intakeDeployer, RobotContainer robotContainer) {
         this.drivebase = drivebase;
         this.robotContainer = robotContainer;
         this.previousState = getCurrentShootState();
         this.activeTargets = STOPPED_TARGETS;
+        this.intakeDeployer = intakeDeployer;
 
         SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0);
         SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0);
@@ -86,6 +93,7 @@ public void periodic() {
         Pose2d robotPose = getRobotPose();
         ShootState currentState = getCurrentShootState();
         updateStopDelayState(currentState);
+        updateShootDelayState(currentState);
         updateTargets(currentState, robotPose);
         if (Constants.DEBUG) {
             SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString());
@@ -141,7 +149,16 @@ private void updateStopDelayState(ShootState currentState) {
         }
     }
 
+    private void updateShootDelayState(ShootState currentState) {
+        if (currentState != ShootState.STOPPED && previousState != currentState) {
+            shootDelayTimer.restart();
+        }
+    }
+
     private void updateTargets(ShootState state, Pose2d robotPose) {
+        if(!activeTargets.intakeDeploy && intakeDeployer.getDeploymentState() == DeploymentState.DOWN){
+            new ToggleDeployment(intakeDeployer, this).schedule();
+        }
         switch (state) {
             case STOPPED -> updateStoppedTargets();
             case FIXED -> useShotTargets(FIXED_TARGETS);
@@ -159,7 +176,6 @@ private void updateTargets(ShootState state, Pose2d robotPose) {
                     useShotTargets(FIXED_TARGETS);
                 }
             }
-
             case SHUTTLING -> {
                 if (Robot.allianceColor().isEmpty()) {
                     useShotTargets(FIXED_TARGETS);
@@ -186,18 +202,37 @@ private void updateStoppedTargets() {
                 STOPPED_TARGETS.turretAngleDegrees,
                 STOPPED_TARGETS.distanceMeters,
                 STOPPED_TARGETS.feederSpin,
-                STOPPED_TARGETS.hopperSpin);
+                STOPPED_TARGETS.hopperSpin,
+                STOPPED_TARGETS.intakeDeploy);
     }
 
     private void useShotTargets(ShotTargets shotTargets) {
-        boolean driverEnabled = driverActivatedShootingEnabled();
-        activeTargets = new ShotTargets(
+
+        // This makes everything wait until after the shooter has run for half a second before starting
+        if (shootDelayTimer.hasElapsed(SHOOT_DELAY_SECONDS)) {
+
+            activeTargets = new ShotTargets(
                 shotTargets.anglerAngleDegrees,
                 shotTargets.shooterVelocityRpm,
                 shotTargets.turretAngleDegrees,
                 shotTargets.distanceMeters,
-                driverEnabled,
-                driverEnabled);
+                shotTargets.hopperSpin,
+                shotTargets.feederSpin,
+                shotTargets.intakeDeploy);
+
+        } else {
+
+            activeTargets = new ShotTargets(
+                activeTargets.anglerAngleDegrees,
+                shotTargets.shooterVelocityRpm, // Shooter starts half a second before everything else
+                activeTargets.turretAngleDegrees,
+                activeTargets.distanceMeters,
+                false,
+                false,
+                activeTargets.intakeDeploy);
+
+        }
+
     }
 
     private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose) {
@@ -206,7 +241,7 @@ private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile
         double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile);
         double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile);
         return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true,
-                true);
+                true, true);
     }
 
     private double calculateDistanceMeters(ShootState state,Pose2d robotPose, Pose2d targetPose) {
@@ -296,6 +331,10 @@ public boolean shouldHopperSpin() {
         return activeTargets.hopperSpin;
     }
 
+    public boolean canIntakeDeploy() {
+        return activeTargets.intakeDeploy;
+    }
+
     public void setDriverActivatedShooting(boolean set) {
         driverActivatedShooting = set;
     }
@@ -312,20 +351,22 @@ private static final class ShotTargets {
         private final double distanceMeters;
         private final boolean feederSpin;
         private final boolean hopperSpin;
+        private final boolean intakeDeploy;
 
         private ShotTargets(
                 double anglerAngleDegrees,
                 double shooterVelocityRpm,
                 double turretAngleDegrees,
                 double distanceMeters,
                 boolean feederSpin,
-                boolean hopperSpin) {
+                boolean hopperSpin, boolean intakeDeploy) {
             this.anglerAngleDegrees = anglerAngleDegrees;
             this.shooterVelocityRpm = shooterVelocityRpm;
             this.turretAngleDegrees = turretAngleDegrees;
             this.distanceMeters = distanceMeters;
             this.feederSpin = feederSpin;
             this.hopperSpin = hopperSpin;
+            this.intakeDeploy = intakeDeploy;
         }
     }
 

src/main/java/frc/robot/subsystems/IntakeDeployerSubsystem.java

@@ -35,7 +35,6 @@ public class IntakeDeployerSubsystem extends SubsystemBase {
 
   public IntakeDeployerSubsystem(SparkMaxIo io) {
     this.io = io;
-    setDefaultCommand(new RunDeployer(this));
   }
 
   public void setSpeed(double speed) {
@@ -103,8 +102,8 @@ public void setDeploymentState(DeploymentState deploymentState) {
     this.deploymentState = deploymentState;
   }
 
-  public void toggleState() {
-    if (deploymentState == DeploymentState.UP) {
+  public void toggleState(boolean canDeploy) {
+    if (deploymentState == DeploymentState.UP && canDeploy) {
       deploymentState = DeploymentState.DOWN;
     } else {
       deploymentState = DeploymentState.UP;