Add tests for command changes during deceleration

Add four tests to verify hysteresis behavior when the target position
changes during deceleration:

1. CommandChangeDuringDecel_TargetFurtherOut: Moving target further out
   causes eventual switch to acceleration and reaches new target.

2. CommandChangeDuringDecel_TargetCloser: Moving target closer causes
   continued deceleration (with overshoot) then return to new target.

3. CommandChangeDuringDecel_SmallIncrease: Small target increases during
   late deceleration are handled correctly.

4. CommandChangeDuringDecel_HysteresisEffect: Documents that hysteresis
   causes continued deceleration for some steps even when switching to
   acceleration would be valid.

Author: claude

fw/test/bldc_servo_position_test.cc

@@ -740,3 +740,223 @@ BOOST_AUTO_TEST_CASE(ControlAccelerationConsistent) {
   BOOST_TEST(end_of_trajectory_violations <= 15);
   BOOST_TEST(zero_violations == 0);
 }
+
+// Test that changing the target position during deceleration works correctly.
+// When target is moved further out, we should switch to acceleration.
+BOOST_AUTO_TEST_CASE(CommandChangeDuringDecel_TargetFurtherOut,
+                     * boost::unit_test::tolerance(1e-3)) {
+  Context ctx;
+
+  constexpr float kAccel = 1000.0f;
+  constexpr float kInitialTarget = 0.5f;
+  constexpr float kNewTarget = 1.0f;  // Further out
+
+  ctx.data.position = kInitialTarget;
+  ctx.data.accel_limit = kAccel;
+  ctx.data.velocity_limit = NaN;
+  ctx.rate_hz = 10000.0f;
+
+  ctx.set_velocity(0.0f);
+  ctx.set_position(0.0f);
+
+  // For a 0.5 rev trajectory with a=1000:
+  // - v_peak = sqrt(2*1000*0.25) ≈ 22.4 rev/s
+  // - t_peak = v_peak/a = 0.0224s = 224 steps at 10kHz
+  // Run until we're well into deceleration phase
+  int step = 0;
+  for (; step < 280; step++) {
+    ctx.Call();
+  }
+
+  // Verify we're decelerating (negative acceleration, positive velocity)
+  BOOST_TEST(ctx.status.control_acceleration < 0.0f);
+  BOOST_TEST(ctx.status.control_velocity.value() > 0.0f);
+
+  // Now move the target further out
+  ctx.data.position = kNewTarget;
+  ctx.data.position_relative_raw.reset();  // Force recalculation
+
+  // Continue running - we should eventually switch to acceleration
+  bool switched_to_accel = false;
+  for (; step < 10000; step++) {
+    ctx.Call();
+
+    if (!switched_to_accel && ctx.status.control_acceleration > 0.0f) {
+      switched_to_accel = true;
+    }
+
+    if (ctx.status.trajectory_done) {
+      break;
+    }
+  }
+
+  BOOST_TEST(switched_to_accel);
+  // Should reach the new target
+  BOOST_TEST(ctx.from_raw(ctx.status.control_position_raw.value()) == kNewTarget);
+  BOOST_TEST(ctx.status.trajectory_done == true);
+}
+
+// Test that changing the target position closer during deceleration works.
+// We should continue decelerating, overshoot, then accelerate back.
+BOOST_AUTO_TEST_CASE(CommandChangeDuringDecel_TargetCloser,
+                     * boost::unit_test::tolerance(1e-3)) {
+  Context ctx;
+
+  constexpr float kAccel = 1000.0f;
+  constexpr float kInitialTarget = 0.5f;
+  constexpr float kNewTarget = 0.3f;  // Closer
+
+  ctx.data.position = kInitialTarget;
+  ctx.data.accel_limit = kAccel;
+  ctx.data.velocity_limit = NaN;
+  ctx.rate_hz = 10000.0f;
+
+  ctx.set_velocity(0.0f);
+  ctx.set_position(0.0f);
+
+  // Run until we're well into deceleration phase
+  int step = 0;
+  for (; step < 280; step++) {
+    ctx.Call();
+  }
+
+  // Verify we're decelerating
+  BOOST_TEST(ctx.status.control_acceleration < 0.0f);
+  BOOST_TEST(ctx.status.control_velocity.value() > 0.0f);
+
+  // Move the target closer
+  ctx.data.position = kNewTarget;
+  ctx.data.position_relative_raw.reset();  // Force recalculation
+
+  // Continue running - we should continue decelerating (hysteresis),
+  // overshoot the new target, then come back.
+  bool ever_overshot = false;
+
+  for (; step < 10000; step++) {
+    ctx.Call();
+    float pos = ctx.from_raw(ctx.status.control_position_raw.value());
+
+    if (pos > kNewTarget + 0.001f) {
+      ever_overshot = true;
+    }
+
+    if (ctx.status.trajectory_done) {
+      break;
+    }
+  }
+
+  // Should overshoot since we were going fast when target moved closer
+  BOOST_TEST(ever_overshot);
+  // Should reach the new target
+  BOOST_TEST(ctx.from_raw(ctx.status.control_position_raw.value()) == kNewTarget);
+  BOOST_TEST(ctx.status.trajectory_done == true);
+}
+
+// Test that small position changes during deceleration are handled correctly.
+// A small increase in target position should still eventually reach it.
+BOOST_AUTO_TEST_CASE(CommandChangeDuringDecel_SmallIncrease,
+                     * boost::unit_test::tolerance(1e-3)) {
+  Context ctx;
+
+  constexpr float kAccel = 1000.0f;
+  constexpr float kInitialTarget = 0.5f;
+  // Small increase - just a few decel distances worth
+  constexpr float kSmallDelta = 0.02f;
+  constexpr float kNewTarget = kInitialTarget + kSmallDelta;
+
+  ctx.data.position = kInitialTarget;
+  ctx.data.accel_limit = kAccel;
+  ctx.data.velocity_limit = NaN;
+  ctx.rate_hz = 10000.0f;
+
+  ctx.set_velocity(0.0f);
+  ctx.set_position(0.0f);
+
+  // Run until we're very late in deceleration phase (low velocity)
+  // At step 400, we're near the end of a ~448 step trajectory
+  int step = 0;
+  for (; step < 400; step++) {
+    ctx.Call();
+  }
+
+  // Verify we're decelerating with relatively low velocity
+  BOOST_TEST(ctx.status.control_acceleration < 0.0f);
+  const float vel_at_change = ctx.status.control_velocity.value();
+  BOOST_TEST(vel_at_change > 0.0f);
+  BOOST_TEST(vel_at_change < 10.0f);  // Should be slowing down significantly
+
+  // Small increase in target
+  ctx.data.position = kNewTarget;
+  ctx.data.position_relative_raw.reset();  // Force recalculation
+
+  // Continue and verify we reach the new target
+  for (; step < 10000; step++) {
+    ctx.Call();
+
+    if (ctx.status.trajectory_done) {
+      break;
+    }
+  }
+
+  BOOST_TEST(ctx.from_raw(ctx.status.control_position_raw.value()) == kNewTarget);
+  BOOST_TEST(ctx.status.trajectory_done == true);
+}
+
+// Test behavior when hysteresis prevents immediate acceleration switch.
+// With a position increase during deceleration, the hysteresis causes
+// continued deceleration for a number of steps.
+BOOST_AUTO_TEST_CASE(CommandChangeDuringDecel_HysteresisEffect,
+                     * boost::unit_test::tolerance(1e-3)) {
+  Context ctx;
+
+  constexpr float kAccel = 1000.0f;
+  constexpr float kInitialTarget = 0.5f;
+  constexpr float kNewTarget = 0.6f;  // 0.1 rev further
+
+  ctx.data.position = kInitialTarget;
+  ctx.data.accel_limit = kAccel;
+  ctx.data.velocity_limit = NaN;
+  ctx.rate_hz = 10000.0f;
+
+  ctx.set_velocity(0.0f);
+  ctx.set_position(0.0f);
+
+  // Run until we're decelerating but still have significant velocity
+  // At step 280, we're in the middle of deceleration
+  int step = 0;
+  for (; step < 280; step++) {
+    ctx.Call();
+  }
+
+  BOOST_TEST(ctx.status.control_acceleration < 0.0f);
+  const float vel_before = ctx.status.control_velocity.value();
+  BOOST_TEST(vel_before > 5.0f);  // Still significant velocity
+
+  // Move target further out
+  ctx.data.position = kNewTarget;
+  ctx.data.position_relative_raw.reset();  // Force recalculation
+
+  // Track acceleration changes after the command change
+  int steps_still_decelerating = 0;
+
+  for (; step < 10000; step++) {
+    ctx.Call();
+    float vel = ctx.status.control_velocity.value();
+
+    if (ctx.status.control_acceleration < 0.0f && vel > 0.0f) {
+      steps_still_decelerating++;
+    }
+
+    if (ctx.status.trajectory_done) {
+      break;
+    }
+  }
+
+  // Due to hysteresis, we should continue decelerating for some steps
+  // even though we could theoretically switch to acceleration immediately.
+  BOOST_TEST(steps_still_decelerating > 0);
+
+  // Should still reach the new target
+  BOOST_TEST(ctx.from_raw(ctx.status.control_position_raw.value()) == kNewTarget);
+  BOOST_TEST(ctx.status.trajectory_done == true);
+}