diff --git a/comp/src/main/java/org/team100/frc2026/auton/LeftSequenceExample.java b/comp/src/main/java/org/team100/frc2026/auton/LeftSequenceExample.java index af361ab..937348d 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/LeftSequenceExample.java +++ b/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 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 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), diff --git a/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java b/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java index f16e777..b4ede4f 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/RightTrenchLeave.java +++ b/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)); diff --git a/comp/src/main/java/org/team100/frc2026/util/AutoWinner.java b/comp/src/main/java/org/team100/frc2026/util/AutoWinner.java new file mode 100644 index 0000000..97d6f41 --- /dev/null +++ b/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 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(); + } + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/util/Hub.java b/comp/src/main/java/org/team100/frc2026/util/Hub.java new file mode 100644 index 0000000..133ddcd --- /dev/null +++ b/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 currentShift = Shift.current(); + if (currentShift.isEmpty()) + return false; + Optional alliance = DriverStation.getAlliance(); + if (alliance.isEmpty()) + return false; + return currentShift.get().active(alliance.get()); + } +} diff --git a/comp/src/main/java/org/team100/frc2026/util/MatchTime.java b/comp/src/main/java/org/team100/frc2026/util/MatchTime.java new file mode 100644 index 0000000..7ea59bb --- /dev/null +++ b/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(); + } +} diff --git a/comp/src/main/java/org/team100/frc2026/util/Shift.java b/comp/src/main/java/org/team100/frc2026/util/Shift.java new file mode 100644 index 0000000..038ea4c --- /dev/null +++ b/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 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 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 + } +} diff --git a/lib/src/test/java/org/team100/lib/optimization/GoldenSectionSearchTest.java b/lib/src/test/java/org/team100/lib/optimization/GoldenSectionSearchTest.java index 558e125..ec76ca7 100644 --- a/lib/src/test/java/org/team100/lib/optimization/GoldenSectionSearchTest.java +++ b/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 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(); diff --git a/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java b/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java index 263e1cf..d01bd9a 100644 --- a/lib/src/test/java/org/team100/lib/optimization/GradientDescentTest.java +++ b/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, Double> f = (x) -> Math.pow((x.get(0) - 1), 2) + 1; GradientDescent 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 axis = VecBuilder.fill(0, 0, 1); diff --git a/lib/src/test/java/org/team100/lib/optimization/NewtonsMethodTest.java b/lib/src/test/java/org/team100/lib/optimization/NewtonsMethodTest.java index 52af4c9..240dc41 100644 --- a/lib/src/test/java/org/team100/lib/optimization/NewtonsMethodTest.java +++ b/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 Xd = VecBuilder.fill(0, 1); Vector q0 = VecBuilder.fill(0, Math.PI / 2); diff --git a/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java b/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java index 99f8e25..f3d2365 100644 --- a/lib/src/test/java/org/team100/lib/optimization/TernarySearchTest.java +++ b/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 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(); diff --git a/lib/src/test/java/org/team100/lib/profile/r1/IncrementalProfileTest.java b/lib/src/test/java/org/team100/lib/profile/r1/IncrementalProfileTest.java index 5f1468f..0ff986b 100644 --- a/lib/src/test/java/org/team100/lib/profile/r1/IncrementalProfileTest.java +++ b/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); diff --git a/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfileTest.java b/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfileTest.java index 3960439..5940a81 100644 --- a/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfileTest.java +++ b/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; diff --git a/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidProfileWPITest.java b/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidProfileWPITest.java index ce5a105..68faff6 100644 --- a/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidProfileWPITest.java +++ b/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; diff --git a/lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java b/lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java index 23a3303..ad8087b 100644 --- a/lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java +++ b/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)); diff --git a/lib/src/test/java/org/team100/lib/targeting/RangeSolverTest.java b/lib/src/test/java/org/team100/lib/targeting/RangeSolverTest.java index 02ffbf7..115baae 100644 --- a/lib/src/test/java/org/team100/lib/targeting/RangeSolverTest.java +++ b/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); diff --git a/lib/src/test/java/org/team100/lib/targeting/RangeTest.java b/lib/src/test/java/org/team100/lib/targeting/RangeTest.java index ab5779c..18dc4da 100644 --- a/lib/src/test/java/org/team100/lib/targeting/RangeTest.java +++ b/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); diff --git a/lib/src/test/java/org/team100/lib/targeting/ShootingMethodTest.java b/lib/src/test/java/org/team100/lib/targeting/ShootingMethodTest.java index 76d1620..06b7fdd 100644 --- a/lib/src/test/java/org/team100/lib/targeting/ShootingMethodTest.java +++ b/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); diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectorySE2Test.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectorySE2Test.java index f47a3c7..4035ab3 100644 --- a/lib/src/test/java/org/team100/lib/trajectory/TrajectorySE2Test.java +++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectorySE2Test.java @@ -161,7 +161,8 @@ private void check(TrajectorySE2 trajectory, double t, double x) { * * There's no need to run this all the time */ - @Test + // disable to speed up tests + // @Test void testSamplePerformance() { WaypointSE2 p0 = new WaypointSE2(new Pose2d(new Translation2d(), Rotation2d.kZero), new DirectionSE2(1, 0, 0), 1);