PunkHPX8/Framework/Common/Algorithm/PdiAlgorithm.cs

using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace MECF.Framework.Common.Algorithm
{
    public class PdiAlgorithm : Singleton<PdiAlgorithm>
    {
        #region 常量
        private const double RANGE_PRESSURE = 2;
        #endregion
        #region 内部变量
        /// <summary>
        /// 差额系数
        /// </summary>
        private double _kp;
        /// <summary>
        /// 积分系数
        /// </summary>
        private double _ki;
        /// <summary>
        /// 微分系数
        /// </summary>
        private double _kd;
        /// <summary>
        /// 上一次偏差
        /// </summary>
        private double _lastBias;
        /// <summary>
        /// 积分值
        /// </summary>
        private double _integral;
        /// <summary>
        /// 目标压力
        /// </summary>
        private double _targetPressure;
        /// <summary>
        /// 最大偏差
        /// </summary>
        private double _maxBias = 0;
        #endregion
        /// <summary>
        /// 计算速度
        /// </summary>
        /// <param name="pressure"></param>
        /// <param name="speed"></param>
        /// <returns></returns>
        public short CalculateSpeed(double kp, double ki, double kd, double targetPressure, double pressure, short speed, double targetLimit,
            double downTargetLimit, double minSpeedDelta)
        {
            _kp = kp;
            _ki = ki;
            _kd = kd;
            _targetPressure = targetPressure;
            _maxBias = 0.5 * targetPressure;
            double index = 0;
            double bias = _targetPressure - pressure;
            if (bias >= 0 && bias <= targetLimit)
            {
                return speed;
            }
            if (bias >= -downTargetLimit && bias < 0)
            {
                return speed;
            }

            _integral += bias;
            if (Math.Abs(bias) > _maxBias)
            {
                index = 0;
            }
            else if (Math.Abs(bias) < 0.9 * _maxBias)
            {
                index = 1;
                _integral += bias;
            }
            else
            {
                index = (_maxBias - Math.Abs(bias)) / (0.9 * _maxBias);
                _integral += bias;
            }

            double pressureBias = _kp * bias + index * _ki * _integral + _kd * (bias - _lastBias);
            double range = SC.GetValue<double>("VPWMain.RangePressure");
            if (Math.Abs(bias) >= range)
            {
                if (bias < 0)
                {
                    pressureBias = -minSpeedDelta;
                }
                else
                {
                    pressureBias = minSpeedDelta;
                }
            }
            _lastBias = bias;
            short finalSpeed = (short)Math.Round(speed + pressureBias, 0);
            int maxPumpSpeed = SC.GetValue<int>("VPWMain.MaxPumpSpeed");
            int minPumpSpeed = SC.GetValue<int>("VPWMain.MinPumpSpeed");
            if (finalSpeed >= maxPumpSpeed)
            {
                finalSpeed = (short)maxPumpSpeed;
            }
            else if (finalSpeed <= minPumpSpeed)
            {
                finalSpeed = (short)minPumpSpeed;
            }
            return finalSpeed;
        }

        public short CalculateSpeed(double targetPressure, double pressure, short speed, double targetLimit,
            double downTargetLimit)
        {
            //根据现场手动调速得到的3次拟合多项式拟合系统 y=k3x^3+K2x^2+k2x+offset(y=0.8641x^3-131.23x^2+6750.4x-113116) 拟合r2为0.9929
            //Pressure	Pre-wet Pump Speed
            //37.9    1000
            //38  1300
            //38.6    1600
            //38.9    1900
            //39.3    2200
            //40.6    2500
            //41.6    2800
            //42.5    3100
            //44.3    3400
            //45.2    3700
            //47.9    4000
            //50.4    4300
            //51.5    4600
            //54.04   4900
            //56.13   5200
            //58.12   5500

            double k3 = 0.8641;
            double k2 = -131.23;
            double k1 = 6750.4;
            double bias = _targetPressure - pressure;
            if (bias >= 0 && bias <= targetLimit)
            {
                return speed;
            }
            if (bias >= -downTargetLimit && bias < 0)
            {
                return speed;
            }
            double speedDelta = k3 * Math.Pow(targetPressure, 3) + k2 * Math.Pow(targetPressure, 2) + k1 * targetPressure -
                k3 * Math.Pow(pressure, 3) - k2 * Math.Pow(pressure, 2) - k1 * pressure;
            short finalSpeed = (short)Math.Round(speed + speedDelta, 0);
            int maxPumpSpeed = SC.GetValue<int>("Prewet.MaxPumpSpeed");
            int minPumpSpeed = SC.GetValue<int>("Prewet.MinPumpSpeed");
            if (finalSpeed >= maxPumpSpeed)
            {
                finalSpeed = (short)maxPumpSpeed;
            }
            else if (finalSpeed <= minPumpSpeed)
            {
                finalSpeed = (short)minPumpSpeed;
            }
            return finalSpeed;
        }
    }
}