frc::NormalizeAngle(), units::math::NormalizeAngle(), and
frc::GetModulusError() were replaced with frc::InputModulus() and
frc::AngleModulus().
They were placed in wpimath/src/main/native/include/frc/MathUtil.h for
C++ and wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
for Java.
This makes AtSetpoint() return false after the setpoint is changed with
SetSetpoint().
Closes#2821.
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
There were three bugs:
1. The input range variables used in ProfiledPIDController::Calculate()
weren't being updated
2. The modulus error calculation was incorrect.
3. The setpoint wasn't being wrapped like the goal, so the invariant
that the error remains less than half the input range was violated.
(Thanks to @CptJJ for pointing this out and suggesting a fix.)
pose.Translation().X() and pose.Translation.Y() are common operations,
so shortening them to pose.X() and pose.Y() would be convenient.
Java uses the getX() convention so that is used instead of X() for Java.
Previously, it could take the long way around. This recomputes the
profile goal with the shortest error, thus taking the shortest path.
Also removed the setpoint clamping from PIDController::SetSetpoint()
because it's unnecessary to make PIDController behave correctly for
a modular arithmetic input, and it breaks the setpoint calculation in
ProfiledPIDController otherwise.
Fixes#2277.
If users are attempting to use the output range to limit the controller
action, they should use ProfiledPIDController instead. If they actually
intended to clamp the output, they should use std::clamp().
It breaks the unit system badly; the tolerance member variable has
different units depending on percent vs absolute. Absolute tolerance is
a lot more natural than percent tolerance anyway.
