[wpimath] Add ElevatorFeedforward.calculate(currentV, nextV) overload

[narmstro2020]

The Elevator and Arm FF controllers were missing a method of the form
public double calculate(double currentVelocity, double nextVelocity, double dtSeconds)
The Arm FF controller has one with a positionRadians component at the beginning.

Java version only at this point.

[calcmogul]

The arm implementation doesn't provide an exact solution. See #5192 (comment).

[narmstro2020]

Would page 106 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf
help in this

[calcmogul]

No, because we want uₖ for some function xₖ₊₁ = f_d(xₖ, uₖ). We only have dx/dt = f(x) + Bu. My comment on the other issue suggests a numerical method since there is no analytical discrete model for an arm.

[narmstro2020]

Okay. So the arm needs work. Is the Elevator FF good?

[calcmogul]

I think so, but we should include the derivation in an internal comment. It's also worth noting that if x crosses zero during the timestep, the computed input will be wrong.

[narmstro2020]

You mean this derivation
dx/dt = Ax + Bu + c
dx/dt = Ax + B(u + B⁺c)
xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ

Solve for uₖ.

B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ

For an elevator with the model dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka).

uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka))
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka)
uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks

---

I'm not sure what you mean when x crosses the zero. Do you mean when the elevator's position reaches bottom

Also should I remove the ArmFF from the pull request. I'm not quite sure how to do that

[calcmogul]

I mean when the velocity crosses zero, because the friction force changes direction and makes the model nonlinear instead of affine (sgn(x) is a nonlinear function in that regime).

[calcmogul]

The elevator derivation should also include sgn(x) and state the assumption that x doesn't cross zero during the step, making sgn(x) a constant.

You could remove the arm feedforward part from this PR. Also revert the comment formatting changes, because javaformat is complaining about them.

A C++ version will be needed as well, of course.

[narmstro2020]

Ah okay. I see what you mean now.

So could that be broken up into a sum of two movements. Like going from 2 to -2. Chop it up into from 2 to 0 and 0 to -2 and handle the 0 separately

[calcmogul]

Yes. It's a small enough effect in a rare enough situation though that I'd be fine just leaving a comment about it.

[narmstro2020]

Okay. I've committed a reversion of the ArmFF.

Still trying to find the issues with the comment formatting in the Elevator.

How do I submit and internal comment with the derivation?

[calcmogul]

Add a sequence of single-line comments at the top of the function body. Longer derivations would go in https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/algorithms.md.

[narmstro2020]

Like this

[calcmogul]

No, the function body is the contents of the function.

[narmstro2020]

Sorry. It's late. I inferred above the method signature

[narmstro2020]

Thank you @calcmogul for helping me out with this. First time submitting a pull request since I started working with our team 3 years ago.

[narmstro2020]

I'm stuck on the p tag at line 62 of ElevFF. Can't seem to format it correctly

[calcmogul]

I'll clean up the C++ for you.

[narmstro2020]

Thank you. I tried. I really tried.

Definitely has been a learning experience. Thank you