The main problem: you are assigning your derivative only if the integrator isn't locked, but you don't set the derivative to zero if it is locked, so the derivative remains at the last value set before being locked.
To further improve the implementation:
- Use two separate lock states inLowerLimit and inUpperLimit
- Leave the lock states if the input is negative (for inUpperLimit) or positive (for inLowerLimit)
- Limit the continuous state instead of just the output, otherwise you'll end up with numeric errors