mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
This avoids large additions introduced by the D term when a step change occurs in the setpoint. Otherwise, the changes return the same values. Let error = setpoint - input and prevError = prevSetpoint - prevInput. If the D term is calculated via error - prevError, then:
error - prevError = (setpoint - input) - (prevSetpoint - prevInput)
If we ignore the setpoint changing, then we get:
error - prevError = (setpoint - input) - (setpoint - prevInput)
= prevInput - input
Change-Id: Ifa4af9b265e3c4bd263e8541355f2b80269693e9