Пример #1
0
        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;
        }
Пример #2
0
        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;
        }