Feature/stow update

src/main/java/frc/robot/Constants.java

@@ -43,6 +43,8 @@ public static class Drive {
 
             public static final double TARGET_TRANSLATION_RADIUS = 2.0;
 
+            public static final double SHOOTING_SPEED_SCALE = 0.5; // Scale factor applied to drive speed while shooting
+
         }
 
         public static class ErrorSettings {
@@ -390,6 +392,11 @@ public static class Turret {
         public static final Rotation2d HOOD_START_POSITION = Rotation2d.fromDegrees(5.00);
         public static final Rotation2d HOOD_STOW_POSITION = Rotation2d.fromDegrees(3.00);
 
+        public static final double HOOD_STABLE_TIME = 0.1;
+        public static final double HOOD_STABLE = 5;
+
+        public static final double HOOD_FINISH_VELOCITY = 0.0;
+
         public static final double OUTPUT_RANGE_MAX = 1;
         public static final double OUTPUT_RANGE_MIN = -1;
 
@@ -460,6 +467,8 @@ public static class Transfer {
         public static final boolean MOTOR_INVERTED = true;
 
         public static final double LOAD_POWER = 0.25;
+
+        public static final double LOAD_DELAY = 0.1; //Time between shooter and transfer being triggered, shooter should always activate first
     }
 
     public static class VisionOdometryConstants {

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

@@ -268,6 +268,9 @@ public void configureBindings() {
 
     Trigger shootTrigger = new Trigger(() -> driverController.getAButton());
 
+    // Limit drive speed to 50% while the shooting button is held
+    driveSubsystem.setSpeedScaleSupplier(() -> driverController.getAButton() ? Constants.Operator.Drive.SHOOTING_SPEED_SCALE : 1.0);
+
     Trigger intakeTrigger = new Trigger(() -> driverController.getRightBumperButton());
 
     Trigger solverValid = new Trigger(() -> TurretSolver.solve(driveSubsystem.getPose(), driveSubsystem.getVelocity(),
@@ -284,10 +287,21 @@ public void configureBindings() {
             driveSubsystem::getPose,
             driveSubsystem::getVelocity));
 
+    shootTrigger.whileTrue(
+      turretSubsystem.getTargetCommand(
+        targetingSupplier,
+        driveSubsystem::getPose,
+        driveSubsystem::getVelocity
+      )
+    );
+
     /* Transfer runs ONLY while button AND solver valid */
     shootTrigger
         .whileTrue(
-            transferSubsystem.getLoadCommand());
+          new WaitCommand(Constants.Transfer.LOAD_DELAY).andThen(
+            transferSubsystem.getLoadCommand()
+          ));
+
 
     /* Intake pivot runs while button held */
     intakeTrigger.whileTrue(
@@ -515,10 +529,7 @@ public void scheduleTeleOp() {
         manualRotationVelocityDirective, null, null);
     driveSubsystem.setDefaultCommand(manualDriveCommand);
 
-    turretSubsystem.setDefaultCommand(turretSubsystem.getTargetCommand(
-        targetingSupplier,
-        driveSubsystem::getPose,
-        driveSubsystem::getVelocity));
+    turretSubsystem.setDefaultCommand(turretSubsystem.getStowCommand());
 
   }
 

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

@@ -58,6 +58,9 @@ public class DriveSubsystem extends SubsystemBase {
     // Feature Flags 
     private boolean useVisionOdometry = true;
 
+    // Speed scaling (e.g. 0.5 to limit to 50% while shooting)
+    private Supplier<Double> speedScaleSupplier = () -> 1.0;
+
     public DriveSubsystem() {
 
         SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
@@ -190,6 +193,16 @@ public void removeObstaclesSupplier(Supplier<List<Obstacle>> supplier) {
         obstaclesSuppliers.remove(supplier);
     }
 
+    /**
+     * Sets a supplier that returns a speed scale factor applied to all drive output.
+     * A value of 1.0 is full speed; 0.5 limits the robot to 50% of normal speed.
+     *
+     * @param supplier Supplier returning a scale factor in the range [0.0, 1.0].
+     */
+    public void setSpeedScaleSupplier(Supplier<Double> supplier) {
+        this.speedScaleSupplier = (supplier != null) ? supplier : () -> 1.0;
+    }
+
     // ------------------------------
     // POSE ACCESS
     // ------------------------------
@@ -300,6 +313,12 @@ public void driveWithCompositeRequests(TranslationRequest tReq, RotationRequest
             );
         }
 
+        // Apply speed scale (e.g. 50% while shooting)
+        double scale = MathUtil.clamp(speedScaleSupplier.get(), 0.0, 1.0);
+        chassisSpeeds.vxMetersPerSecond *= scale;
+        chassisSpeeds.vyMetersPerSecond *= scale;
+        chassisSpeeds.omegaRadiansPerSecond *= scale;
+
         // --- Drive: field-oriented unless robot-relative velocity ---
         if (tReq instanceof TranslationRequest.Velocity vel && !vel.fieldRelative()) {
             swerveDrive.drive(chassisSpeeds); // robot-relative velocity

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

@@ -21,6 +21,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -168,6 +169,10 @@ public Rotation2d getHoodAngle() {
                         - hoodOffsetSupplier.get().getRadians());
     }
 
+    public double getHoodVelocity() {
+        return pitchEncoder.getVelocity().getValueAsDouble() * Constants.Turret.PITCH_ENCODER_FACTOR;
+    }
+
     /**
      * @return the current turret yaw in the field frame
      */
@@ -269,32 +274,52 @@ public Command getAngleCommand(
 
     public Command getStowCommand() {
 
-        Command moveToTarget = new FunctionalCommand(
-                () -> SmartDashboard.putBoolean("Turret/IsStowing", true),
-
-                () -> {
-                    setHoodAngle(Constants.Turret.HOOD_STOW_POSITION);
-                },
-
-                interrupted -> {
-                },
+        final Timer settleTimer = new Timer();
+        double stopThreshold = Math.max(1e-3, Math.abs(Constants.Turret.HOOD_FINISH_VELOCITY)); // rad/s
+        double maxSettleTime = Constants.Turret.HOOD_STABLE_TIME; // seconds, or a separate constant
 
-                () -> Math.abs(
+        Command moveToStow = Commands.run(
+                () -> setHoodAngle(Constants.Turret.HOOD_STOW_POSITION),
+                this)
+                .until(() -> Math.abs(
                         getHoodAngle()
                                 .minus(Constants.Turret.HOOD_STOW_POSITION)
-                                .getRadians()) < Constants.Turret.HOOD_TOLERANCE.getRadians(),
-
-                this);
-
-        Command settleDown = new RunCommand(
-                () -> pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED), // small constant downward speed
-                this).withTimeout(Constants.Turret.STOW_PUSH_DOWN_TIME); // enough to seat the gear
-
+                                .getRadians()) < Constants.Turret.HOOD_TOLERANCE.getRadians())
+                .finallyDo(interrupted -> stopHoodServo());
+
+        Command settleDown = Commands.run(
+                () -> pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED),
+                this)
+            // stop when encoder velocity magnitude <= threshold
+            .until(() -> {
+                boolean within = Math.abs(getHoodVelocity()) <= stopThreshold;
+
+                if (within) {
+                    if (!settleTimer.isRunning()) {
+                        settleTimer.reset();
+                        settleTimer.start();
+                    }
+                    //Finish when considered stable
+                    return settleTimer.hasElapsed(maxSettleTime);
+                } else {
+                    settleTimer.stop();
+                    settleTimer.reset();
+                    return false;
+                }
+            }
+            )
+            // ensure the servo is stopped when this command completes
+            .andThen(() -> {
+                pitchServo.setSpeed(0.0);
+                settleTimer.stop();
+                settleTimer.reset();
+            }
+            );
         Command finish = new InstantCommand(() -> {
             pitchServo.setSpeed(0.0);
         });
 
-        return Commands.sequence(moveToTarget, settleDown, finish, Commands.idle())
+        return Commands.sequence(moveToStow, settleDown, finish, Commands.idle())
                 .withName("StowHood");
     }