void ControlLoop(double newValue)
{
//ignore sensor updates as long as the controller is not running
//or if we are in unrecoverable integral fault
if (!_isRunning || _integralFault)
{
return;
}
//approach setpoint to target
if (_setPoint > _target)
{
_setPoint = (_setPoint - _updateStep > _target) ? _setPoint - _updateStep : _target;
}
else if (_setPoint < _target)
{
_setPoint = (_setPoint + _updateStep < _target) ? _setPoint + _updateStep : _target;
}
//if we currently have a limit fault but are within limits now we recover - otherwise we exit
//at this point
if (_limitFault)
{
if (newValue <= _maxSystemValue && newValue >= _minSystemValue)
{
_limitFault = false;
_effector.Start();
System.Diagnostics.Debug.WriteLine("Recovered from limit fault in controller" + _id);
}
else
{
return;
}
}
//detect new system limit condition
if (newValue > _maxSystemValue || newValue < _minSystemValue)
{
System.Diagnostics.Debug.WriteLine("Limit fault detected in controller {0}. Value = {1}.", _id, newValue);
_limitFault = true;
//temporarily stop the effector and exit
_effector.Stop();
return;
}
//compute current error
double error = _setPoint - newValue;
_integralOfError += error;
//detect integral fault
if (Math.Abs(_integralOfError) > _maxIntegralError)
{
System.Diagnostics.Debug.WriteLine("Integral fault detected in controller {0}. Integral error = {1}. Shutting down controller", _id, _integralOfError);
_effector.StopOnFatalFault();
_integralFault = true;
return;
}
//update the output on the effector
_effector.Output = error * _kP + _integralOfError * _kI;
}
void ControlLoop(double newValue)
{
//ignore sensor updates as long as the controller is not running
//or if we are in unrecoverable integral fault
if (!_isRunning || _integralFault)
{
return;
}
//approach setpoint to target
if (_setPoint > _target)
{
_setPoint = (_setPoint - _updateStep > _target) ? _setPoint - _updateStep : _target;
}
else if (_setPoint < _target)
{
_setPoint = (_setPoint + _updateStep < _target) ? _setPoint + _updateStep : _target;
}
//if we currently have a limit fault but are within limits now we recover - otherwise we exit
//at this point
if (_limitFault)
{
if (newValue <= _maxSystemValue && newValue >= _minSystemValue)
{
_limitFault = false;
_effector.Start();
System.Diagnostics.Debug.WriteLine("Recovered from limit fault in controller" + _id);
}
else
{
return;
}
}
//detect new system limit condition
if (newValue > _maxSystemValue || newValue < _minSystemValue)
{
System.Diagnostics.Debug.WriteLine("Limit fault detected in controller {0}. Value = {1}.", _id, newValue);
_limitFault = true;
//temporarily stop the effector and exit
_effector.Stop();
return;
}
//compute current error
double error = _setPoint - newValue;
_integralOfError += error;
//compute error on delta constraint - this error is only negative if we are within
//the acceptable range - so the integral can grow out of bounds if the acceptable
//value is set too stringent effectively disabling the controller!
double errorDelta = Math.Abs(_delta.Value) - _acceptableDelta;
_constraintIOE += errorDelta;
//our integral of the constraint should not get negative!
if (_constraintIOE < 0)
{
_constraintIOE = 0;
}
//detect integral fault
if (Math.Abs(_integralOfError) > _maxIntegralError)
{
System.Diagnostics.Debug.WriteLine("Integral fault detected in controller {0}. Integral error = {1}. Shutting down controller", _id, _integralOfError);
_effector.StopOnFatalFault();
_integralFault = true;
return;
}
//a negative errorDelta should not impact the output
if (errorDelta < 0)
{
errorDelta = 0;
}
//update the output on the effector
_effector.Output = error * _kP + _integralOfError * _kI - errorDelta * _kPConstraint - _constraintIOE * _kIConstraint;
}
