Merge branch 'wpilibsuite:main' into main

epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java

@@ -269,12 +269,18 @@ private boolean isNotLoggable(Element element, TypeMirror type) {
       return false;
     }
 
-    processingEnv
-        .getMessager()
-        .printMessage(
-            Diagnostic.Kind.NOTE,
-            "[EPILOGUE] Excluded from logs because " + type + " is not a loggable data type",
-            element);
+    var classConfig = element.getEnclosingElement().getAnnotation(Logged.class);
+
+    if (classConfig == null || classConfig.warnForNonLoggableTypes()) {
+      // Not loggable and not explicitly opted out of logging; print a warning message
+      processingEnv
+          .getMessager()
+          .printMessage(
+              Diagnostic.Kind.NOTE,
+              "[EPILOGUE] Excluded from logs because " + type + " is not a loggable data type",
+              element);
+    }
+
     return true;
   }
 

epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java

@@ -71,6 +71,11 @@ public Importance importance() {
         public Naming defaultNaming() {
           return Naming.USE_CODE_NAME;
         }
+
+        @Override
+        public boolean warnForNonLoggableTypes() {
+          return false;
+        }
       };
 
   public LoggerGenerator(ProcessingEnvironment processingEnv, List<ElementHandler> handlers) {

epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java

@@ -1772,7 +1772,7 @@ void warnsAboutNonLoggableFields() {
         """
         package edu.wpi.first.epilogue;
 
-        @Logged
+        @Logged(warnForNonLoggableTypes = true)
         class Example {
           Throwable t;
         }

epilogue-runtime/src/main/java/edu/wpi/first/epilogue/Logged.java

@@ -124,4 +124,12 @@ enum Naming {
    *     for all logged fields and methods in an annotated class
    */
   Naming defaultNaming() default Naming.USE_CODE_NAME;
+
+  /**
+   * Class-level only: if {@link #strategy()} is {@link Strategy#OPT_OUT}, this can be used to quiet
+   * the warnings that are printed for non-loggable fields and methods detected within the class.
+   *
+   * @return true if warnings should be printed, or false if warnings should not be printed
+   */
+  boolean warnForNonLoggableTypes() default false;
 }

wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h

@@ -52,11 +52,11 @@ class LinearSystemSim {
    * @param dt The time between updates.
    */
   void Update(units::second_t dt) {
-    // Update x. By default, this is the linear system dynamics x_k+1 = Ax_k +
-    // Bu_k
+    // Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ +
+    // Buₖ.
     m_x = UpdateX(m_x, m_u, dt);
 
-    // y = Cx + Du
+    // yₖ = Cxₖ + Duₖ
     m_y = m_plant.CalculateY(m_x, m_u);
 
     // Add noise. If the user did not pass a noise vector to the
@@ -115,7 +115,14 @@ class LinearSystemSim {
    *
    * @param state The new state.
    */
-  void SetState(const Vectord<States>& state) { m_x = state; }
+  void SetState(const Vectord<States>& state) {
+    m_x = state;
+
+    // Update the output to reflect the new state.
+    //
+    //   yₖ = Cxₖ + Duₖ
+    m_y = m_plant.CalculateY(m_x, m_u);
+  }
 
  protected:
   /**

wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp

@@ -44,6 +44,15 @@ TEST(ElevatorSimTest, StateSpaceSim) {
   EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().value(), 0.2);
 }
 
+TEST(ElevatorSimTest, InitialState) {
+  constexpr auto startingHeight = 0.5_m;
+  frc::sim::ElevatorSim sim(frc::DCMotor::KrakenX60(2), 20, 8_kg, 0.1_m, 0_m,
+                            1_m, true, startingHeight, {0.01, 0.0});
+
+  EXPECT_DOUBLE_EQ(startingHeight.value(), sim.GetPosition().value());
+  EXPECT_DOUBLE_EQ(0, sim.GetVelocity().value());
+}
+
 TEST(ElevatorSimTest, MinMax) {
   frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, 0.75_in,
                             0_m, 1_m, true, 0_m, {0.01});

wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp

@@ -21,3 +21,12 @@ TEST(SingleJointedArmTest, Disabled) {
   // The arm should swing down.
   EXPECT_NEAR(sim.GetAngle().value(), -std::numbers::pi / 2, 0.01);
 }
+
+TEST(SingleJointedArmTest, InitialState) {
+  constexpr auto startingAngle = 45_deg;
+  frc::sim::SingleJointedArmSim sim(frc::DCMotor::KrakenX60(2), 125, 3_kg_sq_m,
+                                    30_in, 0_deg, 90_deg, true, startingAngle);
+
+  EXPECT_EQ(startingAngle, sim.GetAngle());
+  EXPECT_DOUBLE_EQ(0, sim.GetVelocity().value());
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java

@@ -75,10 +75,10 @@ public LinearSystemSim(
    * @param dtSeconds The time between updates.
    */
   public void update(double dtSeconds) {
-    // Update X. By default, this is the linear system dynamics X = Ax + Bu
+    // Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ + Buₖ.
     m_x = updateX(m_x, m_u, dtSeconds);
 
-    // y = cx + du
+    // yₖ = Cxₖ + Duₖ
     m_y = m_plant.calculateY(m_x, m_u);
 
     // Add measurement noise.
@@ -164,6 +164,11 @@ public double getInput(int row) {
    */
   public void setState(Matrix<States, N1> state) {
     m_x = state;
+
+    // Update the output to reflect the new state.
+    //
+    //   yₖ = Cxₖ + Duₖ
+    m_y = m_plant.calculateY(m_x, m_u);
   }
 
   /**

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java

@@ -63,6 +63,17 @@ void testStateSpaceSimWithElevator() {
     }
   }
 
+  @Test
+  void testInitialState() {
+    double startingHeightMeters = 0.5;
+    var sim =
+        new ElevatorSim(
+            DCMotor.getKrakenX60(2), 20, 8.0, 0.1, 0.0, 1.0, true, startingHeightMeters, 0.01, 0.0);
+
+    assertEquals(startingHeightMeters, sim.getPositionMeters());
+    assertEquals(0, sim.getVelocityMetersPerSecond());
+  }
+
   @Test
   void testMinMax() {
     var sim =

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java

@@ -12,28 +12,46 @@
 import org.junit.jupiter.api.Test;
 
 class SingleJointedArmSimTest {
-  SingleJointedArmSim m_sim =
-      new SingleJointedArmSim(
-          DCMotor.getVex775Pro(2),
-          300,
-          3.0,
-          Units.inchesToMeters(30.0),
-          -Math.PI,
-          0.0,
-          true,
-          Math.PI / 2.0);
-
   @Test
   void testArmDisabled() {
+    SingleJointedArmSim sim =
+        new SingleJointedArmSim(
+            DCMotor.getVex775Pro(2),
+            300,
+            3.0,
+            Units.inchesToMeters(30.0),
+            -Math.PI,
+            0.0,
+            true,
+            Math.PI / 2.0);
+
     // Reset Arm angle to 0
-    m_sim.setState(VecBuilder.fill(0.0, 0.0));
+    sim.setState(VecBuilder.fill(0.0, 0.0));
 
     for (int i = 0; i < 12 / 0.02; i++) {
-      m_sim.setInput(0.0);
-      m_sim.update(0.020);
+      sim.setInput(0.0);
+      sim.update(0.020);
     }
 
     // the arm should swing down
-    assertEquals(-Math.PI / 2.0, m_sim.getAngleRads(), 0.1);
+    assertEquals(-Math.PI / 2.0, sim.getAngleRads(), 0.1);
+  }
+
+  @Test
+  void testInitialState() {
+    double startingAngleRads = Math.PI / 4.0;
+    SingleJointedArmSim sim =
+        new SingleJointedArmSim(
+            DCMotor.getKrakenX60(2),
+            125,
+            3.0,
+            Units.inchesToMeters(30.0),
+            0,
+            Math.PI / 2.0,
+            true,
+            startingAngleRads);
+
+    assertEquals(startingAngleRads, sim.getAngleRads());
+    assertEquals(0, sim.getVelocityRadPerSec());
   }
 }