CyberX8/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 内部变量
        /// <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)
        {
            _kp = kp;
            _ki = ki;
            _kd = kd;
            _targetPressure = targetPressure;
            _maxBias = 0.5 * targetPressure;
            double index = 0;
            double bias=_targetPressure - pressure;
            if(Math.Abs(bias)<=targetLimit)
            {
                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);
            //根据伯努利一般形态P+1/2V^2=C
            double speedBias =Math.Round(Math.Sqrt(2 * pressureBias),2);
            _lastBias=bias;
            short finalSpeed= (short)Math.Round(speed+pressureBias,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;
        }
    }
}