Added output constraints and a way to change the setpoint.
Changed to measurement based derivative and a few other things.
Should be pretty much fully functional at this point.