active hub util classes, also disabled slow tests

comp/src/main/java/org/team100/frc2026/auton/LeftSequenceExample.java

@@ -39,7 +39,7 @@ public LeftSequenceExample(
             SwerveKinodynamics kinodynamics,
             ControllerSE2 controller,
             Machinery machinery) {
-        log = parent.name("RightTrenchLeave");
+        log = parent.name(name());
         this.controller = controller;
         this.machinery = machinery;
         constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this));
@@ -56,45 +56,31 @@ public String name() {
     // go to the middle while spinning 90 degrees
     TrajectorySE2 t1(Pose2d startingPose) {
         List<WaypointSE2> waypoints = List.of(
-                new WaypointSE2(
-                        startingPose,
-                        new DirectionSE2(-1, 0, 0),
-                        1),
-                new WaypointSE2(
-                        new Pose2d(2, 4, Rotation2d.kCW_Pi_2),
-                        new DirectionSE2(0, -1, 0),
-                        1));
+                new WaypointSE2(startingPose,
+                        new DirectionSE2(-1, 0, 0), 1),
+                new WaypointSE2(new Pose2d(2, 4, Rotation2d.kCW_Pi_2),
+                        new DirectionSE2(0, -1, 0), 1));
         return planner.restToRest(waypoints);
     }
 
     // go back where we started.
     TrajectorySE2 t2(Pose2d startingPose) {
         List<WaypointSE2> waypoints = List.of(
-                new WaypointSE2(
-                        startingPose,
-                        new DirectionSE2(0, 1, 0),
-                        1),
-                new WaypointSE2(
-                        StartingPositions.LEFT_TRENCH,
-                        new DirectionSE2(1, 0, 0),
-                        1));
+                new WaypointSE2(startingPose,
+                        new DirectionSE2(0, 1, 0), 1),
+                new WaypointSE2(StartingPositions.LEFT_TRENCH,
+                        new DirectionSE2(1, 0, 0), 1));
         return planner.restToRest(waypoints);
     }
 
     @Override
     public Command command() {
         DriveWithTrajectoryFunction n1 = new DriveWithTrajectoryFunction(
-                log,
-                machinery.m_drive,
-                controller,
-                machinery.m_trajectoryViz,
-                this::t1);
+                log, machinery.m_drive, controller,
+                machinery.m_trajectoryViz, this::t1);
         DriveWithTrajectoryFunction n2 = new DriveWithTrajectoryFunction(
-                log,
-                machinery.m_drive,
-                controller,
-                machinery.m_trajectoryViz,
-                this::t2);
+                log, machinery.m_drive, controller,
+                machinery.m_trajectoryViz, this::t2);
         return sequence(
                 n1.until(n1::isDone),
                 waitSeconds(1),

comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java

@@ -41,7 +41,7 @@ public RightTrenchLeave(
             SwerveKinodynamics kinodynamics,
             ControllerSE2 controller,
             Machinery machinery) {
-        log = parent.name("RightTrenchLeave");
+        log = parent.name(name());
         this.controller = controller;
         this.machinery = machinery;
         constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this));

comp/src/main/java/org/team100/frc2026/util/AutoWinner.java

@@ -0,0 +1,32 @@
+package org.team100.frc2026.util;
+
+import java.util.Optional;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+/**
+ * The FMS tells us who won auto, so we can deduce whose shift it is.
+ * 
+ * see https://gist.github.com/LordOfFrogs/240ba37cf696ba156d87f387c1461bd5
+ */
+public class AutoWinner {
+    /**
+     * Returns the Alliance that won auto as specified by the FMS/Driver
+     * Station's game specific message data, or empty if no game message or alliance
+     * is available.
+     */
+    public static Optional<Alliance> get() {
+        String msg = DriverStation.getGameSpecificMessage();
+        char msgChar = msg.length() > 0 ? msg.charAt(0) : ' ';
+        switch (msgChar) {
+            case 'B':
+                return Optional.of(Alliance.Blue);
+            case 'R':
+                return Optional.of(Alliance.Red);
+            default:
+                return Optional.empty();
+        }
+    }
+
+}

comp/src/main/java/org/team100/frc2026/util/Hub.java

@@ -0,0 +1,23 @@
+package org.team100.frc2026.util;
+
+import java.util.Optional;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+public class Hub {
+    /**
+     * Can we score currently?
+     * 
+     * See 2026 rule 6.4.1.
+     */
+    public static boolean active() {
+        Optional<Shift> currentShift = Shift.current();
+        if (currentShift.isEmpty())
+            return false;
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        if (alliance.isEmpty())
+            return false;
+        return currentShift.get().active(alliance.get());
+    }
+}

comp/src/main/java/org/team100/frc2026/util/MatchTime.java

@@ -0,0 +1,26 @@
+package org.team100.frc2026.util;
+
+import java.util.OptionalDouble;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * Counts up from 0 to 160 seconds, or empty if match isn't running, or if
+ * between auto and teleop.
+ * 
+ * see https://gist.github.com/LordOfFrogs/240ba37cf696ba156d87f387c1461bd5
+ */
+public class MatchTime {
+    public static OptionalDouble get() {
+        double matchTime = DriverStation.getMatchTime();
+        if (matchTime < 0)
+            return OptionalDouble.empty();
+        if (DriverStation.isAutonomous()) {
+            return OptionalDouble.of(20 - matchTime);
+        }
+        if (DriverStation.isTeleop()) {
+            return OptionalDouble.of(160 - matchTime);
+        }
+        return OptionalDouble.empty();
+    }
+}

comp/src/main/java/org/team100/frc2026/util/Shift.java

@@ -0,0 +1,63 @@
+package org.team100.frc2026.util;
+
+import java.util.Optional;
+import java.util.OptionalDouble;
+
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+/**
+ * Describes the "shifts" in the 2026 game, and which one is "active" based on
+ * the auton winner.
+ * 
+ * see https://gist.github.com/LordOfFrogs/240ba37cf696ba156d87f387c1461bd5
+ */
+public enum Shift {
+    AUTO(0, 20, ActiveType.BOTH),
+    TRANSITION(20, 30, ActiveType.BOTH),
+    SHIFT_1(30, 55, ActiveType.AUTO_LOSER),
+    SHIFT_2(55, 80, ActiveType.AUTO_WINNER),
+    SHIFT_3(80, 105, ActiveType.AUTO_LOSER),
+    SHIFT_4(105, 130, ActiveType.AUTO_WINNER),
+    ENDGAME(130, 160, ActiveType.BOTH);
+
+    final int startTime;
+    final int endTime;
+    final ActiveType activeType;
+
+    /** Return the current shift, or empty if no valid shift */
+    static Optional<Shift> current() {
+        OptionalDouble matchTime = MatchTime.get();
+        if (matchTime.isEmpty())
+            return Optional.empty();
+
+        for (Shift shift : Shift.values()) {
+            if (matchTime.getAsDouble() < shift.endTime) {
+                return Optional.of(shift);
+            }
+        }
+        return Optional.empty();
+    }
+
+    /** Is this shift active, given the auto winner? */
+    boolean active(Alliance alliance) {
+        Optional<Alliance> autoWinner = AutoWinner.get();
+        return switch (activeType) {
+            case BOTH -> true;
+            case AUTO_WINNER -> autoWinner.isPresent() && autoWinner.get() == alliance;
+            case AUTO_LOSER -> autoWinner.isPresent() && autoWinner.get() != alliance;
+            default -> false;
+        };
+    }
+
+    private Shift(int startTime, int endTime, ActiveType activeType) {
+        this.startTime = startTime;
+        this.endTime = endTime;
+        this.activeType = activeType;
+    }
+
+    private enum ActiveType {
+        BOTH,
+        AUTO_WINNER,
+        AUTO_LOSER
+    }
+}

lib/src/test/java/org/team100/lib/optimization/GoldenSectionSearchTest.java

@@ -85,7 +85,8 @@ void testPose() {
     }
 
     /** 8 us per solve on my laptop */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testPosePerformance() {
         Pose3d desired = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, 1));
         Vector<N3> axis = VecBuilder.fill(0, 0, 1);
@@ -138,7 +139,8 @@ void testFasterPose() {
     }
 
     /** 2 us per solve, using the faster metric and 1e-3 tolerance. */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testFasterPosePerformance() {
         Pose3d desired = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, 1));
         Translation3d t0 = desired.getTranslation();

lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java

@@ -32,7 +32,8 @@ void test1() {
     }
 
     /** Not particularly fast, 2 us per solve on my laptop. */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testPerformance() {
         Function<Vector<N1>, Double> f = (x) -> Math.pow((x.get(0) - 1), 2) + 1;
         GradientDescent<N1> s = new GradientDescent<>(Nat.N1(), f, 1e-3, 100);
@@ -85,7 +86,8 @@ void testPose() {
     }
 
     /** 31 us per solve on my laptop, slower than ternary. */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testPosePerformance() {
         Pose3d desired = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, 1));
         Vector<N3> axis = VecBuilder.fill(0, 0, 1);

lib/src/test/java/org/team100/lib/optimization/NewtonsMethodTest.java

@@ -582,7 +582,8 @@ void test6() {
     }
 
     /** 4 us per solve */
-    @Test
+        // disable to speed up tests
+    // @Test
     void test7() {
         Vector<N2> Xd = VecBuilder.fill(0, 1);
         Vector<N2> q0 = VecBuilder.fill(0, Math.PI / 2);

lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java

@@ -48,7 +48,8 @@ void testBimodal() {
     }
 
     /** 0.1 us per solve on my laptop */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testPerformance() {
         DoubleUnaryOperator f = (x) -> Math.pow((x - 1), 2) + 1;
         TernarySearch s = new TernarySearch(f, 1e-3, 100);
@@ -85,7 +86,8 @@ void testPose() {
     }
 
     /** about 20 us per solve on my laptop */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testPosePerformance() {
         Pose3d desired = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, 1));
         Vector<N3> axis = VecBuilder.fill(0, 0, 1);
@@ -138,7 +140,8 @@ void testFasterPose() {
     }
 
     /** 3.5 us per solve, using the faster metric and 1e-3 tolerance. */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testFasterPosePerformance() {
         Pose3d desired = new Pose3d(new Translation3d(1, 1, 1), new Rotation3d(0, 0, 1));
         Translation3d t0 = desired.getTranslation();

lib/src/test/java/org/team100/lib/profile/r1/IncrementalProfileTest.java

@@ -16,7 +16,8 @@ public class IncrementalProfileTest implements Timeless {
     private final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger());
 
     // 70 ns to get the ETA
-    @Test
+    // disable to speed up tests
+    // @Test
     void testETA() {
         ControlR1 initial = new ControlR1();
         ModelR1 goal = new ModelR1(1, 0);
@@ -44,7 +45,8 @@ void testETA() {
 
     // 3 us to get the eta at full resolution.
     // 10x coarser step means only 0.5 us
-    @Test
+    // disable to speed up tests
+    // @Test
     void testETA2() {
         ControlR1 initial = new ControlR1();
         ModelR1 goal = new ModelR1(1, 0);
@@ -70,7 +72,8 @@ void testETA2() {
     }
 
     // exponential eta in 1.6 us using coarse step.
-    @Test
+    // disable to speed up tests
+    // @Test
     void testETA2Exponential() {
         ControlR1 initial = new ControlR1();
         ModelR1 goal = new ModelR1(1, 0);

lib/src/test/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfileTest.java

@@ -64,7 +64,8 @@ void testSolve() {
     }
 
     /** Around 25 us at 0.1 DT */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testSolvePerformance() {
         double maxVel = 2;
         double maxAccel = 10;

lib/src/test/java/org/team100/lib/profile/r1/TrapezoidProfileWPITest.java

@@ -67,7 +67,8 @@ void testSolve() {
     }
 
     /** Around 14 us per solve, using DT of 0.1. */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testSolvePerformance() {
         double maxVel = 2;
         double maxAccel = 10;

lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java

@@ -101,7 +101,8 @@ void test2dWithEntrySpeed() {
      * 
      * The SOLVE_DT constant in HolonomicProfile affects performance ~linearly.
      */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testSolvePerformance() {
         HolonomicProfile hp = HolonomicProfileFactory.trapezoidal(logger, 1, 1, 0.01, 1, 1, 0.01);
         ModelSE2 i = new ModelSE2(new Pose2d(), new VelocitySE2(1, 0, 0));

lib/src/test/java/org/team100/lib/targeting/RangeSolverTest.java

@@ -63,7 +63,8 @@ void testDt() {
      * On my machine, which is ~4x faster than a RoboRIO, I get something like 6 us
      * per solve, so the RoboRIO might get 25 us (!).
      */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testPerformance() {
         Drag d = new Drag(0.5, 0, 0.1, 0.1, 0);
         RangeSolver rangeSolver = new RangeSolver(d, 0);

lib/src/test/java/org/team100/lib/targeting/RangeTest.java

@@ -17,7 +17,8 @@ public class RangeTest {
      * 
      * So caching seems like a good idea.
      */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testPerformance() {
         Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1);
         RangeSolver rangeSolver = new RangeSolver(d, 0);

lib/src/test/java/org/team100/lib/targeting/ShootingMethodTest.java

@@ -261,7 +261,8 @@ void testStrafing() {
      * Seems like using the cache is a good idea, especially once we add muzzle
      * velocity to the optimization.
      */
-    @Test
+    // disable to speed up tests
+    // @Test
     void testPerformance() {
         Drag d = new Drag(0.5, 0.025, 0.1, 0.1, 0.1);
         RangeSolver rangeSolver = new RangeSolver(d, 0);