Feature/stow update

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

@@ -45,6 +45,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 Misc {
@@ -407,18 +409,23 @@ public static class Turret {
         public static final double TURN_TABLE_RATIO = 24.0 / 200.0;
         public static final double ENCODER_FACTOR = (TURRET_GEAR_REDUCTION) / (2.0 * Math.PI * TURN_TABLE_RATIO);
 
-        public static final double STOW_PUSH_DOWN_SPEED = -0.3; // percent of max speed
-        public static final double STOW_PUSH_DOWN_TIME = 0.5; // seconds
+        public static final double STOW_PUSH_DOWN_SPEED = 0.6; // percent of max speed
+        public static final double STOW_PUSH_DOWN_TIME = 0.1; // seconds
 
         public static final double PITCH_GEAR_RATIO = (26.0 / 447.2);
         public static final double PITCH_ENCODER_FACTOR = PITCH_GEAR_RATIO * (2.0 * Math.PI);
 
         public static final boolean PITCH_INVERTED = true;
 
         public static final Rotation2d MAX_PITCH = Rotation2d.fromDegrees(45.0);
-        public static final Rotation2d MIN_PITCH = Rotation2d.fromDegrees(3.00);
-        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 Rotation2d MIN_PITCH = Rotation2d.fromDegrees(8.00);
+        public static final Rotation2d HOOD_START_POSITION = Rotation2d.fromDegrees(8.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.1;
+        public static final Rotation2d HOOD_FAR_ANGLE = Rotation2d.fromDegrees(-50.0);
 
         public static final double OUTPUT_RANGE_MAX = 1;
         public static final double OUTPUT_RANGE_MIN = -1;
@@ -521,6 +528,7 @@ public static class Transfer {
         public static final boolean FEEDER_INVERTED = true;
 
 
+        public static final double LOAD_DELAY = 0.1; //Time between shooter and transfer being triggered, shooter should always activate first
         public static final double KICKER_LOAD_POWER = 0.25;
         public static final double FEEDER_LOAD_POWER = 0.5;
     }

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

@@ -267,6 +267,26 @@ public void updateFieldObjects() {
 
   public void configureBindings() {
 
+    new Trigger(
+        () -> operatorController.getAButton()).onTrue(
+            new InstantCommand(() -> selectedFixedTarget = FixedTarget.A));
+
+    new Trigger(
+        () -> operatorController.getBButton()).onTrue(
+            new InstantCommand(() -> selectedFixedTarget = FixedTarget.B));
+
+    new Trigger(
+        () -> operatorController.getXButton()).onTrue(
+            new InstantCommand(() -> xHeld = true))
+        .onFalse(
+            new InstantCommand(() -> xHeld = false));
+
+    new Trigger(
+        () -> operatorController.getYButton()).onTrue(
+            new InstantCommand(() -> yHeld = true))
+        .onFalse(
+            new InstantCommand(() -> yHeld = false));
+
     targetingSupplier = () -> {
       Translation2d robotPosition = driveSubsystem.getPose().getTranslation();
 
@@ -379,6 +399,13 @@ public void configureBindings() {
             driveSubsystem::getPose,
             driveSubsystem::getVelocity));
 
+    /* Turret runs while button held */
+    shootTrigger.whileTrue(
+        turretSubsystem.getTargetCommand(
+            targetingSupplier,
+            driveSubsystem::getPose,
+            driveSubsystem::getVelocity));
+
     /* Transfer runs ONLY while button AND solver valid */
     shootTrigger.whileTrue(
         transferSubsystem.getLoadCommand());
@@ -539,10 +566,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

@@ -22,6 +22,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;
@@ -194,6 +195,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
      */
@@ -336,36 +341,50 @@ public Command getAngleCommand(
 
     public Command getStowCommand() {
 
-        Command moveToTarget = new FunctionalCommand(
-                () -> SmartDashboard.putBoolean("Turret/IsStowing", true),
+        final Timer settleTimer = new Timer();
+        double stopThreshold = Math.max(1e-3, Math.abs(Constants.Turret.HOOD_FINISH_VELOCITY)); // rad/s
+        double stableTime = Constants.Turret.HOOD_STABLE_TIME; // seconds, or a separate constant
 
+        Command settleDown = Commands.run(
                 () -> {
-                    setHoodAngle(Constants.Turret.HOOD_STOW_POSITION);
-                },
-
-                interrupted -> {
+                    pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED);
+                    SmartDashboard.putBoolean("Hood Pushing", true);
                 },
-
-                () -> 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
-
+                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(stableTime);
+                } else {
+                    settleTimer.stop();
+                    settleTimer.reset();
+                    return false;
+                }
+            }
+            ).until(
+                () -> getHoodAngle().getRadians() < Constants.Turret.HOOD_FAR_ANGLE.getRadians()
+            )
+            // ensure the servo is stopped when this command completes
+            .andThen(() -> {
+                pitchServo.setSpeed(0.0);
+                settleTimer.stop();
+                settleTimer.reset();
+            }
+            );
         Command finish = new InstantCommand(() -> {
+            SmartDashboard.putBoolean("Hood Pushing", false);
             pitchServo.setSpeed(0.0);
+            resetHoodAngle(Constants.Turret.HOOD_START_POSITION);
         });
 
-        return Commands.sequence(moveToTarget, settleDown, finish, Commands.idle())
+        return Commands.sequence(settleDown, finish, Commands.idle())
                 .withName("StowHood");
     }