SOTM-centric control upgrades

src/main/deploy/positionDetails.json

@@ -59,27 +59,27 @@
       "yMax": 7.98703
     },
     "blueTrenchLow": {
-      "xMin": 2.5259,
+      "xMin": 3.5259,
       "yMin": -10,
-      "xMax": 6.7197,
+      "xMax": 5.7197,
       "yMax": 1.589881
     },
     "blueTrenchHigh": {
-      "xMin": 2.5259,
+      "xMin": 3.5259,
       "yMin": 6.492081,
-      "xMax": 6.7197,
+      "xMax": 5.7197,
       "yMax": 18.096644
     },
     "redTrenchLow": {
-      "xMin": 9.815700,
+      "xMin": 10.815700,
       "yMin": -10,
-      "xMax": 14.004738,
+      "xMax": 13.004738,
       "yMax": 1.589881
     },
     "redTrenchHigh": {
-      "xMin": 9.815700,
+      "xMin": 10.815700,
       "yMin": 6.492081,
-      "xMax": 14.004738,
+      "xMax": 13.004738,
       "yMax": 18.096644
     }
   }

src/main/java/org/team157/robot/BuildConstants.java

@@ -5,13 +5,13 @@ public final class BuildConstants {
     public static final String MAVEN_GROUP = "";
     public static final String MAVEN_NAME = "FRC2026";
     public static final String VERSION = "unspecified";
-    public static final int GIT_REVISION = 366;
-    public static final String GIT_SHA = "2bdd41328abdfc8999eab7d5cad6e80f4c55241c";
-    public static final String GIT_DATE = "2026-05-15 11:56:30 EDT";
-    public static final String GIT_BRANCH = "task/reexport-as-model";
-    public static final String BUILD_DATE = "2026-05-15 15:57:29 EDT";
-    public static final long BUILD_UNIX_TIME = 1778875049705L;
-    public static final int DIRTY = 0;
+    public static final int GIT_REVISION = 374;
+    public static final String GIT_SHA = "f7f882e99507da8da9d09302734ee9ba52b93c3f";
+    public static final String GIT_DATE = "2026-05-15 16:44:03 EDT";
+    public static final String GIT_BRANCH = "feature/sotm-control";
+    public static final String BUILD_DATE = "2026-05-15 17:43:07 EDT";
+    public static final long BUILD_UNIX_TIME = 1778881387494L;
+    public static final int DIRTY = 1;
 
     private BuildConstants() {}
 }

src/main/java/org/team157/robot/Constants.java

@@ -42,20 +42,15 @@ public static enum Mode {
 
     public static class ModifierConstants {
         // Reduces drive speed by this factor when precision mode is active.
-        public static final double PRECISION_DRIVE_MODIFIER = 0.5;
+        public static final double PRECISION_DRIVE_MODIFIER = 0.4;
+        public static final double TRENCH_DRIVE_MODIFIER = 0.8;
         // When true, reduces drive speed by 50%.
         public static final boolean ROOKIE_MODE = false;
         public static final double ROOKIE_DRIVE_MODIFIER = 0.5;
         // When true, reduces drive speed by 75%.
         // Overrides ROOKIE_MODE.
         public static final boolean DEMO_MODE = false;
         public static final double DEMO_DRIVE_MODIFIER = 0.25;
-        /**
-         * When true, swaps the intake and shooting triggers, per Maya's preference. Makes no change
-         * to drivetrain speed. For future reference, avoid making catches for specific user edge
-         * cases like this, make decisions off general consensus.
-         */
-        public static final boolean MAYA_MODE = false;
     }
 
     public static class ControllerConstants {

src/main/java/org/team157/robot/Robot.java

@@ -114,6 +114,7 @@ public void robotPeriodic() {
         // Gets hub activity status to display on the dashboard.
         Logger.recordOutput("Misc/Hub Active?", m_robotContainer.isHubActive());
         Logger.recordOutput("Misc/Under Trench?", RobotContainer.drive.isUnderTrench());
+        Logger.recordOutput("Flywheel Modifier", RobotContainer.ballisticSpeedModifier);
         m_field.setRobotPose(RobotContainer.drive.getPose());
     }
 

src/main/java/org/team157/robot/RobotContainer.java

@@ -74,6 +74,11 @@ public class RobotContainer {
             RotationsPerSecond.of(0.75)
                     .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
 
+    /**
+     * Speed factor used in flywheel ballistic equations, to be manually adjusted by the operator
+     */
+    public static double ballisticSpeedModifier = 1;
+
     // Subsystems
     public static Vision vision;
     public static Drive drive;
@@ -260,9 +265,9 @@ private void configureBindings() {
         drive.setDefaultCommand(
                 DriveCommands.joystickDrive(
                         drive,
-                        () -> -driverController.getLeftY(),
-                        () -> -driverController.getLeftX(),
-                        () -> -driverController.getRightX()));
+                        () -> modifySpeed(-driverController.getLeftY()),
+                        () -> modifySpeed(-driverController.getLeftX()),
+                        () -> modifySpeed(-driverController.getRightX())));
         // Update the pose estimation and turret tracking angle while no other vision commands are
         // running.
         vision.setDefaultCommand(vision.setDefault(drive, turret));
@@ -306,27 +311,21 @@ private void configureBindings() {
         /////////////////////
         // Enables dynamic control of the flywheel and hood.
         driverController.a().toggleOnTrue(flywheel.setDynamicVelocity());
-        driverController.a().toggleOnTrue(hood.setDynamicHoodAngle());
 
         ////////////////////////////
         /// INTAKE UPTAKE HOPPER ///
         ////////////////////////////
-        // Swaps the intake and shooting triggers if Maya mode is enabled, per Maya's preference.
-        if (ModifierConstants.MAYA_MODE) {
-            // Shooting on left trigger, intake on right trigger
-            driverController.leftTrigger().whileTrue(uptake.set(1));
-            driverController.rightTrigger().whileTrue(intake.runIntake());
-            driverController.leftTrigger().whileTrue(hopper.set(1));
-        } else {
-            // Shooting on right trigger, intake on left trigger
-            driverController.rightTrigger().whileTrue(uptake.set(1));
-            driverController.leftTrigger().whileTrue(intake.runIntake());
-            driverController.rightTrigger().whileTrue(hopper.set(1));
-        }
+        driverController.rightTrigger().whileTrue(uptake.set(1));
+        driverController.leftTrigger().whileTrue(intake.runIntake());
+        driverController.rightTrigger().whileTrue(hopper.set(1));
+
         // Runs the hopper, uptake, and intake backwards at a low speed to clear jams.
         driverController.y().whileTrue(forceOuttake());
         // Wiggles the intake up and down to free up stuck balls
         driverController.x().toggleOnTrue(slapdown.wiggleIntake());
+
+        // (in/de)creases the ballistic modifier
+        operatorController.x().or(operatorController.b()).onTrue(setModifier());
         //////////////////////////////////////////////////
         ///             OPERATOR COMMANDS              ///
         //////////////////////////////////////////////////
@@ -341,7 +340,9 @@ private void configureBindings() {
         // allowing the operator to control the turret without interference from vision tracking.
         turretTrackingTrigger().whileTrue(turret.trackTagGlobalRelative());
         turretTrackingTrigger().whileTrue(flywheel.setDynamicVelocity());
-        turretTrackingTrigger().whileTrue(hood.setDynamicHoodAngle());
+        turretTrackingTrigger()
+                .and(driverController.rightTrigger())
+                .whileTrue(hood.setDynamicHoodAngle());
 
         ///////////////////////
         /// MANUAL FLYWHEEL ///
@@ -389,15 +390,34 @@ private void configureBindings() {
                 .toggleOnTrue(slapdown.retractIntake());
     }
 
-    // If the right bumper is held, apply the precision modifier of 0.5x speed.
+    /**
+     * Apply a speed modifier when the right bumper (dedicated toggle) or shooting trigger are held,
+     * or the robot is under the trench.
+     */
     public double modifySpeed(final double speed) {
-        if (driverController.rightBumper().getAsBoolean() || drive.isUnderTrench()) {
+        if (driverController.rightBumper().getAsBoolean()
+                || driverController.rightTrigger().getAsBoolean()) {
             return speed * ModifierConstants.PRECISION_DRIVE_MODIFIER;
+        } else if (drive.isUnderTrench()) {
+            return speed * ModifierConstants.TRENCH_DRIVE_MODIFIER;
         } else {
             return speed;
         }
     }
 
+    /** Update the ballistic equation modifier based on the operator's button presses */
+    public void setBallisticSpeedModifier() {
+        if (operatorController.x().getAsBoolean()) {
+            ballisticSpeedModifier = ballisticSpeedModifier + 0.05;
+        } else if (operatorController.b().getAsBoolean()) {
+            ballisticSpeedModifier = ballisticSpeedModifier - 0.05;
+        }
+    }
+
+    public InstantCommand setModifier() {
+        return new InstantCommand(() -> setBallisticSpeedModifier());
+    }
+
     public boolean isHubActive() {
         Optional<Alliance> alliance = DriverStation.getAlliance();
         // If we have no alliance, we cannot be enabled, therefore no hub.

src/main/java/org/team157/robot/generated/TunerConstants.java

@@ -56,12 +56,11 @@ public class TunerConstants {
 
     // The stator current at which the wheels start to slip;
     // This needs to be tuned to your individual robot
-    private static final Current kSlipCurrent = Amps.of(120);
+    private static final Current kSlipCurrent = Amps.of(60);
 
     // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
     // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
-    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
-    private static final TalonFXConfiguration steerInitialConfigs =
+    private static final TalonFXConfiguration driveInitialConfigs =
             new TalonFXConfiguration()
                     .withCurrentLimits(
                             new CurrentLimitsConfigs()
@@ -72,6 +71,18 @@ public class TunerConstants {
                                     // impacting performance.
                                     .withStatorCurrentLimit(Amps.of(60))
                                     .withStatorCurrentLimitEnable(true));
+
+    private static final TalonFXConfiguration steerInitialConfigs =
+            new TalonFXConfiguration()
+                    .withCurrentLimits(
+                            new CurrentLimitsConfigs()
+                                    // Swerve azimuth does not require much torque output, so we can
+                                    // set a relatively
+                                    // low
+                                    // stator current limit to help avoid brownouts without
+                                    // impacting performance.
+                                    .withStatorCurrentLimit(Amps.of(40))
+                                    .withStatorCurrentLimitEnable(true));
     private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
     // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
     private static final Pigeon2Configuration pigeonConfigs = null;

src/main/java/org/team157/robot/subsystems/SunstoneMechanism3D.java

@@ -76,7 +76,7 @@ public Transform3d getHoodPivotLocation() {
         return new Transform3d(
                 0.1 * Math.cos(turret.getTurretRotation().in(Radians)),
                 0.1 * Math.sin(turret.getTurretRotation().in(Radians)),
-                0.070,
+                0.064,
                 new Rotation3d(0, 0, turret.getTurretRotation().in(Radians)));
     }
 

src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java

@@ -88,7 +88,7 @@ public Command setVelocity(AngularVelocity speed) {
      * @return {@link Command} continuously updating the flywheel velocity.
      */
     public Command setDynamicVelocity() {
-        return io.setVelocity(this::getDesiredFlywheelVelocity);
+        return io.setVelocity(() -> getDesiredFlywheelVelocity());
     }
 
     ////////////////////////
@@ -188,9 +188,10 @@ public void setShotParams(double height, double distance) {
     }
 
     /**
-     * Gets the desired flywheel velocity for the current shot, recalculating shot parameters each
-     * time it is called.
-     *
+     * Gets the desired flywheel velocity for the current shot, 
+     * recalculating shot parameters each time it is called.
+     * The result of this calculation is multiplied by a modifier controlled by the operator.
+     * 
      * @return The desired angular velocity of the flywheel.
      */
     public AngularVelocity getDesiredFlywheelVelocity() {
@@ -206,7 +207,8 @@ public AngularVelocity getDesiredFlywheelVelocity() {
         double desiredRPM =
                 (ballVelocity * 60)
                         / (Math.PI * flywheelDiameterMeters)
-                        * FlywheelConstants.SPEED_FACTOR;
+                        * FlywheelConstants.SPEED_FACTOR
+                        * RobotContainer.ballisticSpeedModifier;
         return RPM.of(Math.max(2800, desiredRPM));
     }
 

src/main/java/org/team157/robot/subsystems/intake/Intake.java

@@ -4,6 +4,9 @@
 
 package org.team157.robot.subsystems.intake;
 
+import static edu.wpi.first.units.Units.RPM;
+
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
@@ -45,13 +48,24 @@ public Command set(double dutycycle) {
         return io.set(dutycycle);
     }
 
+    /**
+     * Set the intake to a fixed target angular velocity.
+     *
+     * @param speed The target angular velocity.
+     * @return {@link Command} setting the intake to the specified velocity.
+     */
+    public Command setVelocity(AngularVelocity speed) {
+        return io.setVelocity(speed);
+    }
+
     /**
      * Run the intake at a set speed. Used for autonomous and button bindings.
      *
      * @return a {@link Command} running the intake motors at 100% duty cycle
      */
     public Command runIntake() {
-        return set(1);
+        // Arbitrary untuned value
+        return setVelocity(RPM.of(3000));
     }
 
     @Override