using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace SIASUN.Autopilot.BLL
{
    /// <summary>
    /// Pid算法实现
    /// </summary>
    public class PidAlgorithm 
    {
        // 等比系数，微分系数，积分系数
        private double Kp, Ki, Kd;
        //public static PidAlgorithm instance { get; set; }

        //PidAlgorithm()
        //{
        //    instance = new PidAlgorithm();
        //}

         

        public PidAlgorithm(double proportion, double integral, double derivative)
        {
            Kp = proportion;
            Ki = integral;
            Kd = derivative;
        }

        /// <summary>
        /// PID算法实现，把连续函数转化成离散函数
        /// </summary>
        /// <param name="befPid">前一个Pid值</param>
        /// <param name="curVal">当前CTE</param>
        /// <param name="befVal1">前一点的CTE</param>
        /// <param name="befVal2">前两点的CTE</param> 
        public double Pid(double befPid, double curVal, double befVal1, double befVal2)
        {
            double pid = befPid + Kp * (curVal - befVal1) + Ki * curVal + Kd * (curVal - 2 * befVal1 + befVal2);
            return pid;
        }
    }

}
