[wpimath] Fix infinite loop in ArmFeedforward::Calculate(xₖ, vₖ, vₖ₊₁) (

wpilibsuite#7745)

Small values of kₐ make the iterative solver ill-conditioned. This
change reverts to the constant-acceleration feedforward in that case. It
gives _very_ bad results (hence why we added the iterative solver in the
first place), but it's better than hanging.

```
TEST(ArmFeedforwardTest, CalculateIllConditioned) {
  constexpr auto Ks = 0.5_V;
  constexpr auto Kv = 20_V / 1_rad_per_s;
  constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
  constexpr auto Kg = 0_V;
  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};

  // Calculate(currentAngle, currentVelocity, nextAngle, dt)
  CalculateAndSimulate(armFF, 0_rad, 0_rad_per_s, 2_rad_per_s, 20_ms);
}
```
This produces 1 V and doesn't accelerate the system at all. Using
nextVelocity instead of currentVelocity in the feedforward outputs 41 V
and still only accelerates to 0.4 rad/s of the requested 2 rad/s.

I picked the kₐ cutoff by increasing kₐ until the iterative solver
started converging.

Fixes wpilibsuite#7743.

wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp

@@ -20,6 +20,13 @@ units::volt_t ArmFeedforward::Calculate(
     units::unit_t<Velocity> nextVelocity) const {
   using VarMat = sleipnir::VariableMatrix;
 
+  // Small kₐ values make the solver ill-conditioned
+  if (kA < units::unit_t<ka_unit>{1e-1}) {
+    auto acceleration = (nextVelocity - currentVelocity) / m_dt;
+    return kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
+           kA * acceleration + kG * units::math::cos(currentAngle);
+  }
+
   // Arm dynamics
   Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
   Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};