Add stabilization and setpoint-tracking tests for all models

Add test/test_stabilization.jl covering every closed-loop model (continuous
and discrete: angle, cascade, cascade + feed-forward). Each model is simulated
through the DyadInterface.TransientAnalysis path and checked to confirm the
controller actually works, not just that the solve succeeds:

- the body tilt angle stays bounded (the robot never falls over) and settles
  back to ~0 over the final second (the inner loop reaches the upright setpoint);
- for the cascade models, the wheel position tracks the filtered position
  reference at the end of the run (the outer loop reaches its setpoint).

Tolerances are set with comfortable margin over the observed behavior
(max|theta| <= 0.12 vs bound 0.2; settled |theta| <= 0.005 vs 0.02; position
tracking error <= 0.008 vs 0.02). Wired into test/runtests.jl.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01FxiqD9h4podoj5e4tpLeGk

Author: baggepinnen

test/runtests.jl

@@ -30,3 +30,6 @@ end
 end
 
 end
+
+# Self-contained (defines its own `using`s and `@testset`), so include at top level.
+include("test_stabilization.jl")

test/test_stabilization.jl

@@ -0,0 +1,81 @@
+# Closed-loop stabilization and setpoint-tracking tests for every DyadBot model.
+#
+# Each model is simulated through the same `DyadInterface.TransientAnalysis` path
+# the generated case tests use (which applies the SynchToolkit clock pass needed
+# by the discrete models). For every model we check that
+#   * the solver reports success,
+#   * the controller keeps the robot upright — the body tilt angle stays bounded
+#     (never falls over) and settles back to ~0, and
+#   * for the cascade models, the wheel position tracks the filtered position
+#     reference by the end of the run (the outer loop reaches its setpoint).
+
+using DyadBotComponents
+using DiscreteComponents   # loads SynchToolkit so the clocked models get the Lustre pass
+using DyadInterface
+using ModelingToolkit
+using Test
+
+# Simulate a model to `stop` and return (model, solution).
+function simulate_model(constructor; stop)
+    model = constructor(; name = :model)
+    result = TransientAnalysis(; model, alg = DyadInterface.ODEAlg.Auto(),
+        start = 0.0, stop, abstol = 1e-8, reltol = 1e-8,
+        automatic_discontinuity_detection = false)
+    return model, DyadInterface.rebuild_sol(result)
+end
+
+succeeded(sol) = Symbol(sol.retcode) == :Success
+
+# Maximum absolute value of a signal over the whole trajectory.
+maxabs(sol, acc) = maximum(abs, sol[acc])
+
+# Maximum absolute value of `acc` sampled over the time window [t0, t1].
+function maxabs_window(sol, acc, t0, t1)
+    ts = range(t0, t1; length = 50)
+    maximum(abs, sol(ts; idxs = acc).u)
+end
+
+# Maximum absolute tracking error x - r sampled over the window [t0, t1].
+function maxabs_error_window(sol, x, r, t0, t1)
+    ts = range(t0, t1; length = 50)
+    maximum(abs, sol(ts; idxs = x).u .- sol(ts; idxs = r).u)
+end
+
+# The body tilt angle must never exceed this — well below "fallen over".
+const THETA_BOUND = 0.2
+# Settled tolerances, evaluated over the final second of the run.
+const THETA_SETTLE = 0.02
+const POS_TRACK_TOL = 0.02
+
+@testset "Stabilization and setpoint tracking" begin
+    # Angle-only models: the sole setpoint is the upright pose (theta = 0). The
+    # wheel position is intentionally uncontrolled and free to drift.
+    @testset "$(nameof(ctor))" for ctor in (
+        DyadBotComponents.AngleControlledDyadBot,
+        DyadBotComponents.DiscreteAngleControlledDyadBot,
+    )
+        stop = 5.0
+        m, sol = simulate_model(ctor; stop)
+        @test succeeded(sol)
+        @test maxabs(sol, m.plant.theta) < THETA_BOUND          # never falls over
+        @test maxabs_window(sol, m.plant.theta, stop - 1, stop) < THETA_SETTLE  # settles upright
+    end
+
+    # Cascade models: the inner loop holds the robot upright while the outer loop
+    # drives the wheel position to the filtered square-wave reference.
+    @testset "$(nameof(ctor))" for (ctor, reffun) in (
+        (DyadBotComponents.CascadeControlledDyadBot, m -> m.firstorder1.y),
+        (DyadBotComponents.DiscreteCascadeControlledDyadBot, m -> m.firstorder1.y),
+        (DyadBotComponents.CascadeFFDyadBot, m -> m.pos_ref.y),
+        (DyadBotComponents.DiscreteCascadeFFDyadBot, m -> m.pos_ref.y),
+    )
+        stop = 20.0
+        m, sol = simulate_model(ctor; stop)
+        @test succeeded(sol)
+        @test maxabs(sol, m.plant.theta) < THETA_BOUND          # inner loop keeps it upright
+        @test maxabs_window(sol, m.plant.theta, stop - 1, stop) < THETA_SETTLE
+        # Outer loop reaches its setpoint: position tracks the reference at the
+        # end of the run (the reference has been constant for the last 5 s).
+        @test maxabs_error_window(sol, m.plant.x, reffun(m), stop - 1, stop) < POS_TRACK_TOL
+    end
+end