模糊PID控制算法的C#实现

跑起来的效果看每个类的test方法，自己调用来测试

目的是看看哪个算法好用，移植的时候比较单纯没有研究懂算法，代码结构也没改动，只是移植到C#方便查看代码和测试，大家要拷贝也很方便，把整个类拷贝到.cs文件即可

第一段算法来自 模糊PID控制算法的C++实现 ：blog。csdn。net/shuoyueqishilove/article/details/78236541

这段算法在实际值低于目标值是工作正常，超过后会有问题，不知道如何调教

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

namespace FuzzyPID
{
    class FuzzyPID
    {
        public const int N = 7;

        double target; //系统的控制目标
        double actual; //采样获得的实际值
        double e; //误差
        double e_pre_1; //上一次的误差
        double e_pre_2; //上上次的误差
        double de;      //误差的变化率
        double emax;    //误差基本论域上限
        double demax;   //误差辩化率基本论域的上限
        double delta_Kp_max;   //delta_kp输出的上限
        double delta_Ki_max;   //delta_ki输出上限
        double delta_Kd_max;   //delta_kd输出上限
        double Ke;      //Ke=n/emax,量化论域为[-3,-2,-1,0,1,2,3]
        double Kde;     //Kde=n/demax,量化论域为[-3,-2,-1,0,1,2,3]
        double Ku_p;    //Ku_p=Kpmax/n,量化论域为[-3,-2,-1,0,1,2,3]
        double Ku_i;    //Ku_i=Kimax/n,量化论域为[-3,-2,-1,0,1,2,3]
        double Ku_d;    //Ku_d=Kdmax/n,量化论域为[-3,-2,-1,0,1,2,3]
        int[,] Kp_rule_matrix = new int[N, N];//Kp模糊规则矩阵
        int[,] Ki_rule_matrix = new int[N, N];//Ki模糊规则矩阵
        int[,] Kd_rule_matrix = new int[N, N];//Kd模糊规则矩阵
        string mf_t_e;       //e的隶属度函数类型
        string mf_t_de;      //de的隶属度函数类型
        string mf_t_Kp;      //kp的隶属度函数类型
        string mf_t_Ki;      //ki的隶属度函数类型
        string mf_t_Kd;      //kd的隶属度函数类型
        double[] e_mf_paras; //误差的隶属度函数的参数
        double[] de_mf_paras;//误差的偏差隶属度函数的参数
        double[] Kp_mf_paras; //kp的隶属度函数的参数
        double[] Ki_mf_paras; //ki的隶属度函数的参数
        double[] Kd_mf_paras; //kd的隶属度函数的参数
        double Kp;
        double Ki;
        double Kd;
        double A;
        double B;
        double C;

        public FuzzyPID(double e_max, double de_max, double kp_max, double ki_max, double kd_max, double Kp0, double Ki0, double Kd0)
        {
            emax = e_max;
            demax = de_max;
            delta_Kp_max = kp_max;
            delta_Ki_max = ki_max;
            delta_Kd_max = kd_max;
            e = target - actual;
            de = e - e_pre_1;
            Ke = (N / 2) / emax;
            Kde = (N / 2) / demax;
            Ku_p = delta_Kp_max / (N / 2);
            Ku_i = delta_Ki_max / (N / 2);
            Ku_d = delta_Kd_max / (N / 2);
            Kp = Kp0;
            Ki = Ki0;
            Kd = Kd0;
            A = Kp + Ki + Kd;
            B = -2 * Kd - Kp;
            C = Kd;
        }

        //三角隶属度函数
        double trimf(double x, double a, double b, double c)
        {
            double u;
            if (x >= a && x <= b)
                u = (x - a) / (b - a);
            else if (x > b && x <= c)
                u = (c - x) / (c - b);
            else
                u = 0;
            return u;
        }

        //正态隶属度函数
        double gaussmf(double x, double ave, double sigma)
        {
            double u;
            if (sigma < 0)
            {
                throw new Exception("In gaussmf, sigma must larger than 0");
            }
            u = Math.Exp(-Math.Pow(((x - ave) / sigma), 2));
            return u;
        }

        //梯形隶属度函数
        double trapmf(double x, double a, double b, double c, double d)
        {
            double u;
            if (x >= a && x < b)
                u = (x - a) / (b - a);
            else if (x >= b && x < c)
                u = 1;
            else if (x >= c && x <= d)
                u = (d - x) / (d - c);
            else
                u = 0;
            return u;
        }


        //设置模糊规则Matrix
        public void setRuleMatrix(int[,] kp_m, int[,] ki_m, int[,] kd_m)
        {
            for (int i = 0; i < N; i++)
                for (int j = 0; j < N; j++)
                {
                    Kp_rule_matrix[i, j] = kp_m[i, j];
                    Ki_rule_matrix[i, j] = ki_m[i, j];
                    Kd_rule_matrix[i, j] = kd_m[i, j];
                }

        }


        //设置模糊隶属度函数的子函数
        void setMf_sub(string type, double[] paras, int n)
        {
            int N_mf_e = 0, N_mf_de = 0, N_mf_Kp = 0, N_mf_Ki = 0, N_mf_Kd = 0;
            switch (n)
            {
                case 0:
                    if (type == "trimf" || type == "gaussmf" || type == "trapmf")
                        mf_t_e = type;
                    else
                        throw new Exception("Type of membership function must be "trimf" or "gaussmf" or "trapmf"");
                    if (mf_t_e == "trimf")
                        N_mf_e = 3;
                    else if (mf_t_e == "gaussmf")
                        N_mf_e = 2;
                    else if (mf_t_e == "trapmf")
                        N_mf_e = 4;

                    e_mf_paras = new double[N * N_mf_e];
                    for (int i = 0; i < N * N_mf_e; i++)
                        e_mf_paras[i] = paras[i];
                    break;

                case 1:
                    if (type == "trimf" || type == "gaussmf" || type == "trapmf")
                        mf_t_de = type;
                    else
                        throw new Exception("Type of membership function must be "trimf" or "gaussmf" or "trapmf"");
                    if (mf_t_de == "trimf")
                        N_mf_de = 3;
                    else if (mf_t_de == "gaussmf")
                        N_mf_de = 2;
                    else if (mf_t_de == "trapmf")
                        N_mf_de = 4;
                    de_mf_paras = new double[N * N_mf_de];
                    for (int i = 0; i < N * N_mf_de; i++)
                        de_mf_paras[i] = paras[i];
                    break;

                case 2:
                    if (type == "trimf" || type == "gaussmf" || type == "trapmf")
                        mf_t_Kp = type;
                    else
                        throw new Exception("Type of membership function must be "trimf" or "gaussmf" or "trapmf"");
                    if (mf_t_Kp == "trimf")
                        N_mf_Kp = 3;
                    else if (mf_t_Kp == "gaussmf")
                        N_mf_Kp = 2;
                    else if (mf_t_Kp == "trapmf")
                        N_mf_Kp = 4;
                    Kp_mf_paras = new double[N * N_mf_Kp];
                    for (int i = 0; i < N * N_mf_Kp; i++)
                        Kp_mf_paras[i] = paras[i];
                    break;

                case 3:
                    if (type == "trimf" || type == "gaussmf" || type == "trapmf")
                        mf_t_Ki = type;
                    else
                        throw new Exception("Type of membership function must be "trimf" or "gaussmf" or "trapmf"");
                    if (mf_t_Ki == "trimf")
                        N_mf_Ki = 3;
                    else if (mf_t_Ki == "gaussmf")
                        N_mf_Ki = 2;
                    else if (mf_t_Ki == "trapmf")
                        N_mf_Ki = 4;
                    Ki_mf_paras = new double[N * N_mf_Ki];
                    for (int i = 0; i < N * N_mf_Ki; i++)
                        Ki_mf_paras[i] = paras[i];
                    break;

                case 4:
                    if (type == "trimf" || type == "gaussmf" || type == "trapmf")
                        mf_t_Kd = type;
                    else
                        throw new Exception("Type of membership function must be "trimf" or "gaussmf" or "trapmf"");
                    if (mf_t_Kd == "trimf")
                        N_mf_Kd = 3;
                    else if (mf_t_Kd == "gaussmf")
                        N_mf_Kd = 2;
                    else if (mf_t_Kd == "trapmf")
                        N_mf_Kd = 4;
                    Kd_mf_paras = new double[N * N_mf_Kd];
                    for (int i = 0; i < N * N_mf_Kd; i++)
                        Kd_mf_paras[i] = paras[i];
                    break;

                default: break;
            }
        }


        //设置模糊隶属度函数的类型和参数
        public void setMf(string mf_type_e, double[] e_mf,
             string mf_type_de, double[] de_mf,
             string mf_type_Kp, double[] Kp_mf,
             string mf_type_Ki, double[] Ki_mf,
             string mf_type_Kd, double[] Kd_mf)
        {
            setMf_sub(mf_type_e, e_mf, 0);
            setMf_sub(mf_type_de, de_mf, 1);
            setMf_sub(mf_type_Kp, Kp_mf, 2);
            setMf_sub(mf_type_Ki, Ki_mf, 3);
            setMf_sub(mf_type_Kd, Kd_mf, 4);
        }

        //实现模糊控制
        public double realize(double t, double a)
        {
            double[] u_e = new double[N],
                u_de = new double[N],
                u_u = new double[N];
            int[] u_e_index = new int[3], u_de_index = new int[3];//假设一个输入最多激活3个模糊子集
            double delta_Kp, delta_Ki, delta_Kd;
            double delta_u;
            target = t;
            actual = a;
            e = target - actual;
            de = e - e_pre_1;
            e = Ke * e;
            de = Kde * de;
            /* 将误差e模糊化*/
            int j = 0;
            for (int i = 0; i < N; i++)
            {
                if (mf_t_e == "trimf")
                    u_e[i] = trimf(e, e_mf_paras[i * 3], e_mf_paras[i * 3 + 1], e_mf_paras[i * 3 + 2]);//e模糊化，计算它的隶属度
                else if (mf_t_e == "gaussmf")
                    u_e[i] = gaussmf(e, e_mf_paras[i * 2], e_mf_paras[i * 2 + 1]);//e模糊化，计算它的隶属度
                else if (mf_t_e == "trapmf")
                    u_e[i] = trapmf(e, e_mf_paras[i * 4], e_mf_paras[i * 4 + 1], e_mf_paras[i * 4 + 2], e_mf_paras[i * 4 + 3]);//e模糊化，计算它的隶属度

                if (u_e[i] != 0)
                    u_e_index[j++] = i;                //存储被激活的模糊子集的下标，可以减小计算量
            }
            for (; j < 3; j++) u_e_index[j] = 0;             //富余的空间填0

            /*将误差变化率de模糊化*/
            j = 0;
            for (int i = 0; i < N; i++)
            {
                if (mf_t_de == "trimf")
                    u_de[i] = trimf(de, de_mf_paras[i * 3], de_mf_paras[i * 3 + 1], de_mf_paras[i * 3 + 2]);//de模糊化，计算它的隶属度
                else if (mf_t_de == "gaussmf")
                    u_de[i] = gaussmf(de, de_mf_paras[i * 2], de_mf_paras[i * 2 + 1]);//de模糊化，计算它的隶属度
                else if (mf_t_de == "trapmf")
                    u_de[i] = trapmf(de, de_mf_paras[i * 4], de_mf_paras[i * 4 + 1], de_mf_paras[i * 4 + 2], de_mf_paras[i * 4 + 3]);//de模糊化，计算它的隶属度

                if (u_de[i] != 0)
                    u_de_index[j++] = i;            //存储被激活的模糊子集的下标，可以减小计算量
            }
            for (; j < 3; j++) u_de_index[j] = 0;          //富余的空间填0

            double den = 0, num = 0;
            /*计算delta_Kp和Kp*/
            for (int m = 0; m < 3; m++)
                for (int n = 0; n < 3; n++)
                {
                    num += u_e[u_e_index[m]] * u_de[u_de_index[n]] * Kp_rule_matrix[u_e_index[m], u_de_index[n]];
                    den += u_e[u_e_index[m]] * u_de[u_de_index[n]];
                }
            delta_Kp = num / den;
            delta_Kp = Ku_p * delta_Kp;
            if (delta_Kp >= delta_Kp_max) delta_Kp = delta_Kp_max;
            else if (delta_Kp <= -delta_Kp_max) delta_Kp = -delta_Kp_max;
            Kp += delta_Kp;
            if (Kp < 0) Kp = 0;
            /*计算delta_Ki和Ki*/
            den = 0; num = 0;
            for (int m = 0; m < 3; m++)
                for (int n = 0; n < 3; n++)
                {
                    num += u_e[u_e_index[m]] * u_de[u_de_index[n]] * Ki_rule_matrix[u_e_index[m], u_de_index[n]];
                    den += u_e[u_e_index[m]] * u_de[u_de_index[n]];
                }

            delta_Ki = num / den;
            delta_Ki = Ku_i * delta_Ki;
            if (delta_Ki >= delta_Ki_max) delta_Ki = delta_Ki_max;
            else if (delta_Ki <= -delta_Ki_max) delta_Ki = -delta_Ki_max;
            Ki += delta_Ki;
            if (Ki < 0) Ki = 0;
            /*计算delta_Kd和Kd*/
            den = 0; num = 0;
            for (int m = 0; m < 3; m++)
                for (int n = 0; n < 3; n++)
                {
                    num += u_e[u_e_index[m]] * u_de[u_de_index[n]] * Kd_rule_matrix[u_e_index[m], u_de_index[n]];
                    den += u_e[u_e_index[m]] * u_de[u_de_index[n]];
                }
            delta_Kd = num / den;
            delta_Kd = Ku_d * delta_Kd;
            if (delta_Kd >= delta_Kd_max) delta_Kd = delta_Kd_max;
            else if (delta_Kd <= -delta_Kd_max) delta_Kd = -delta_Kd_max;
            Kd += delta_Kd;
            if (Kd < 0) Kd = 0;

            A = Kp + Ki + Kd;
            B = -2 * Kd - Kp;
            C = Kd;
            delta_u = A * e + B * e_pre_1 + C * e_pre_2;

            delta_u = delta_u / Ke;

            if (delta_u >= 0.95 * target) delta_u = 0.95 * target;
            else if (delta_u <= -0.95 * target) delta_u = -0.95 * target;

            e_pre_2 = e_pre_1;
            e_pre_1 = e;

            return delta_u;
        }

        void showMf(string type, double[] mf_paras)
        {
            int tab = 0;
            if (type == "trimf")
                tab = 2;
            else if (type == "gaussmf")
                tab = 1;
            else if (type == "trapmf")
                tab = 3;
            this.WriteLine($"函数类型：{mf_t_e}");
            this.WriteLine("函数参数列表：");
            double[] p = mf_paras;
            for (int i = 0; i < N * (tab + 1); i++)
            {
                this.Write(p[i] + "  ");
                if (i % (tab + 1) == tab)
                    this.Write("
");
            }
        }

        public void showInfo()
        {
            this.WriteLine("Info of this fuzzy controller is as following:");
            this.WriteLine($"基本论域e：[{-emax},{emax}]");
            this.WriteLine($"基本论域de：[{-demax},{demax}]");
            this.WriteLine($"基本论域delta_Kp：[{-delta_Kp_max},{delta_Kp_max}]");
            this.WriteLine($"基本论域delta_Ki：[{-delta_Ki_max},{delta_Ki_max}]");
            this.WriteLine($"基本论域delta_Kd：[{-delta_Kd_max},{delta_Kd_max}]");
            this.WriteLine("误差e的模糊隶属度函数参数：");
            showMf(mf_t_e, e_mf_paras);
            this.WriteLine("误差变化率de的模糊隶属度函数参数：");
            showMf(mf_t_de, de_mf_paras);
            this.WriteLine("delta_Kp的模糊隶属度函数参数：");
            showMf(mf_t_Kp, Kp_mf_paras);
            this.WriteLine("delta_Ki的模糊隶属度函数参数：");
            showMf(mf_t_Ki, Ki_mf_paras);
            this.WriteLine("delta_Kd的模糊隶属度函数参数：");
            showMf(mf_t_Kd, Kd_mf_paras);
            this.WriteLine("模糊规则表：");
            this.WriteLine("delta_Kp的模糊规则矩阵");
            for (int i = 0; i < N; i++)
            {
                for (int j = 0; j < N; j++)
                {
                    this.Write(Kp_rule_matrix[i, j]);
                }
                this.Write("
");
            }
            this.WriteLine("delta_Ki的模糊规则矩阵"); ;
            for (int i = 0; i < N; i++)
            {
                for (int j = 0; j < N; j++)
                {
                    this.WriteLine(Ki_rule_matrix[i, j]);
                }
                WriteEnd();
            }
            this.WriteLine("delta_Kd的模糊规则矩阵"); ;
            for (int i = 0; i < N; i++)
            {
                for (int j = 0; j < N; j++)
                {
                    this.WriteLine(Kd_rule_matrix[i, j]);
                }
                WriteEnd();
            }
            this.WriteLine($"误差的量化比例因子Ke={Ke}");
            this.WriteLine($"误差变化率的量化比例因子Kde={Kde}");
            this.WriteLine($"输出的量化比例因子Ku_p={Ku_p}");
            this.WriteLine($"输出的量化比例因子Ku_i={Ku_i}");
            this.WriteLine($"输出的量化比例因子Ku_d={Ku_d}");
            this.WriteLine($"设定目标target={target}");
            this.WriteLine($"误差e={e}");
            this.WriteLine($"Kp={Kp}");
            this.WriteLine($"Ki={Ki}");
            this.WriteLine($"Kd={Kd}");
            WriteEnd();
        }

        public void Write(object str)
        {
            Console.Write(str);
        }
        public void WriteLine(object str)
        {
            Console.WriteLine(str);
        }
        public void WriteEnd()
        {
            Console.Write("
");
        }


        public static void test()
        {
            int NB = -3;
            int NM = -2;
            int NS = -1;
            int ZO = 0;
            int PS = 1;
            int PM = 2;
            int PB = 3;

            double target = 300;
            double actual = 400;
            double u = 0;

            int[,] deltaKpMatrix = new int[7, 7] {{PB,PB,PM,PM,PS,ZO,ZO
    },
                             {PB,PB,PM,PS,PS,ZO,NS
},
                             {PM,PM,PM,PS,ZO,NS,NS},
                             {PM,PM,PS,ZO,NS,NM,NM},
                             {PS,PS,ZO,NS,NS,NM,NM},
                             {PS,ZO,NS,NM,NM,NM,NB},
                             {ZO,ZO,NM,NM,NM,NB,NB}};
            int[,] deltaKiMatrix = new int[7, 7]{{NB,NB,NM,NM,NS,ZO,ZO},
                             {NB,NB,NM,NS,NS,ZO,ZO},
                             {NB,NM,NS,NS,ZO,PS,PS},
                             {NM,NM,NS,ZO,PS,PM,PM},
                             {NM,NS,ZO,PS,PS,PM,PB},
                             {ZO,ZO,PS,PS,PM,PB,PB},
                             {ZO,ZO,PS,PM,PM,PB,PB}};
            int[,] deltaKdMatrix = new int[7, 7]{{PS,NS,NB,NB,NB,NM,PS},
                             {PS,NS,NB,NM,NM,NS,ZO},
                             {ZO,NS,NM,NM,NS,NS,ZO},
                             {ZO,NS,NS,NS,NS,NS,ZO},
                             {ZO,ZO,ZO,ZO,ZO,ZO,ZO},
                             {PB,NS,PS,PS,PS,PS,PB},
                             {PB,PM,PM,PM,PS,PS,PB}};
            double[] e_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };
            double[] de_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };
            double[] Kp_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };
            double[] Ki_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };
            double[] Kd_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };

            var fuzzypid = new FuzzyPID(1500, 1000, 0.3, 0.9, 0.6, 0.01, 0.04, 0.01);
            fuzzypid.setMf("trimf", e_mf_paras, "trimf", de_mf_paras, "trimf", Kp_mf_paras, "trimf", Ki_mf_paras, "trimf", Kd_mf_paras);
            fuzzypid.setRuleMatrix(deltaKpMatrix, deltaKiMatrix, deltaKdMatrix);

            for (int i = 0; i < 50; i++)
            {
                u = fuzzypid.realize(target, actual);
                actual += u;
                Console.WriteLine($"{i}   {target}  {u}  {actual}");

                //if (i>19)
                //{
                //    target = 300;
                //}
            }
            //fuzzypid.showInfo();

        }

    }
}

第二段来自  模糊PID控制温控系统设计方案C语言程序代码： wenku。baidu。com/view/e62a94fbf342336c1eb91a37f111f18583d00cda.html

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

namespace FuzzyPID
{
    class FuzzyPID2
    {
        int flag;
        int flag_start;
        double S_temp = 60.0;
        double P_temp = 20.0;
        double Kp;
        double Ki;
        double Kd;
        double Err = 0.0;
        double Last_Err = 0.0;
        double D_Err = 0.0;
        double Sum_Err = 0.0;
        double U = 0.0;

        //函数功能：PID的计算
        void PID_Calculate()
        {
            Err = S_temp - P_temp;
            Sum_Err += Err;
            D_Err = Err - Last_Err;
            Last_Err = Err;
            U = Kp * Err + Ki * Sum_Err + Kd * D_Err;
            if (U >= 0)
            {
                if (U >= 200) U = 200;
                flag = 1;
            }
            else
            {
                U = -U;
                if (U >= 200) U = 200;
                flag = 0;
            }
        }

        /***********************************************
           函数功能：PID参数Kp的计算 
  ************************************************/
        double fuzzy_kp(double e, double ec)               //e,ec，表示误差，误差变化率
        {
            double Kp_calcu;
            int num, pe, pec;
            double[] eRule = { -3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0 };   //误差E的模糊论域
            double[] ecRule = { -3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0 }; //误差变化率EC的模糊论域
            double[] eFuzzy = { 0.0, 0.0 };                 //隶属于误差E的隶属程度
            double[] ecFuzzy = { 0.0, 0.0 };            //隶属于误差变化率EC的隶属程度
            double[] kpRule = { 0.0, 8.0, 16.0, 24.0 };         //Kp的模糊子集
            double[] KpFuzzy = { 0.0, 0.0, 0.0, 0.0 };              //隶属于Kp的隶属程度
            int[,] KpRule =                         //Kp的模糊控制表  7, 7
                {
                   { 3,3,3,3,3,3,3 },
                   { 2,2,2,2,1,2,2 },
                   { 1,1,1,1,1,1,1 },
                   { 1,1,0,1,0,1,1 },
                   { 0,0,1,0,0,1,0 },
                   { 0,1,0,1,0,0,2 },
                   { 3,3,3,3,3,3,3 }
                };
            /*****误差E隶属函数描述*****/
            if (e < eRule[0])
            {
                eFuzzy[0] = 1.0;
                pe = 0;
            }
            else if (eRule[0] <= e && e < eRule[1])
            {
                eFuzzy[0] = (eRule[1] - e) / (eRule[1] - eRule[0]);
                pe = 0;
            }
            else if (eRule[1] <= e && e < eRule[2])
            {
                eFuzzy[0] = (eRule[2] - e) / (eRule[2] - eRule[1]);
                pe = 1;
            }
            else if (eRule[2] <= e && e < eRule[3])
            {
                eFuzzy[0] = (eRule[3] - e) / (eRule[3] - eRule[2]);
                pe = 2;
            }
            else if (eRule[3] <= e && e < eRule[4])
            {
                eFuzzy[0] = (eRule[4] - e) / (eRule[4] - eRule[3]);
                pe = 3;
            }
            else if (eRule[4] <= e && e < eRule[5])
            {
                eFuzzy[0] = (eRule[5] - e) / (eRule[5] - eRule[4]);
                pe = 4;
            }
            else if (eRule[5] <= e && e < eRule[6])
            {
                eFuzzy[0] = (eRule[6] - e) / (eRule[6] - eRule[5]);
                pe = 5;
            }
            else
            {
                eFuzzy[0] = 0.0;
                pe = 5;
            }
            eFuzzy[1] = 1.0 - eFuzzy[0];
            /*****误差变化率EC隶属函数描述*****/
            if (ec < ecRule[0])
            {
                ecFuzzy[0] = 1.0;
                pec = 0;
            }
            else if (ecRule[0] <= ec && ec < ecRule[1])
            {
                ecFuzzy[0] = (ecRule[1] - ec) / (ecRule[1] - ecRule[0]);
                pec = 0;
            }
            else if (ecRule[1] <= ec && ec < ecRule[2])
            {
                ecFuzzy[0] = (ecRule[2] - ec) / (ecRule[2] - ecRule[1]);
                pec = 1;
            }
            else if (ecRule[2] <= ec && ec < ecRule[3])
            {
                ecFuzzy[0] = (ecRule[3] - ec) / (ecRule[3] - ecRule[2]);
                pec = 2;
            }
            else if (ecRule[3] <= ec && ec < ecRule[4])
            {
                ecFuzzy[0] = (ecRule[4] - ec) / (ecRule[4] - ecRule[3]);
                pec = 3;
            }
            else if (ecRule[4] <= ec && ec < ecRule[5])
            {
                ecFuzzy[0] = (ecRule[5] - ec) / (ecRule[5] - ecRule[4]);
                pec = 4;
            }
            else if (ecRule[5] <= ec && ec < ecRule[6])
            {
                ecFuzzy[0] = (ecRule[6] - ec) / (ecRule[6] - ecRule[5]);
                pec = 5;
            }
            else
            {
                ecFuzzy[0] = 0.0;
                pec = 5;
            }
            ecFuzzy[1] = 1.0 - ecFuzzy[0];
            /*********查询模糊规则表*********/
            num = KpRule[pe, pec];
            KpFuzzy[num] += eFuzzy[0] * ecFuzzy[0];
            num = KpRule[pe, pec + 1];
            KpFuzzy[num] += eFuzzy[0] * ecFuzzy[1];
            num = KpRule[pe + 1, pec];
            KpFuzzy[num] += eFuzzy[1] * ecFuzzy[0];
            num = KpRule[pe + 1, pec + 1];
            KpFuzzy[num] += eFuzzy[1] * ecFuzzy[1];
            /*********加权平均法解模糊*********/
            Kp_calcu = KpFuzzy[0] * kpRule[0] + KpFuzzy[1] * kpRule[1] + KpFuzzy[2] * kpRule[2]
+ KpFuzzy[3] * kpRule[3];
            return (Kp_calcu);
        }

        /***********************************************
         函数功能：PID参数Ki的计算 
************************************************/
        double fuzzy_ki(double e, double ec)
        {
            double Ki_calcu;
            int num, pe, pec;
            double[] eRule = { -3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0 };
            double[] ecRule = { -3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0 };
            double[] eFuzzy = { 0.0, 0.0 };
            double[] ecFuzzy = { 0.0, 0.0 };
            double[] kiRule = { 0.00, 0.01, 0.02, 0.03 };
            double[] KiFuzzy = { 0.0, 0.0, 0.0, 0.0 };
            int[,] KiRule =
                    {
                        { 0,0,0,0,0,0,0 },
                        { 0,0,0,0,0,0,0 },
                        { 0,0,0,0,0,0,0 },
                        { 0,0,0,0,0,0,0 },
                        { 0,0,0,0,0,0,0 },
                        { 2,0,0,0,0,0,1 },
                        { 3,3,3,3,3,3,3 }

                    };
            /*****误差隶属函数描述*****/
            if (e < eRule[0])
            {
                eFuzzy[0] = 1.0;
                pe = 0;
            }
            else if (eRule[0] <= e && e < eRule[1])
            {
                eFuzzy[0] = (eRule[1] - e) / (eRule[1] - eRule[0]);
                pe = 0;
            }
            else if (eRule[1] <= e && e < eRule[2])
            {
                eFuzzy[0] = (eRule[2] - e) / (eRule[2] - eRule[1]);
                pe = 1;
            }
            else if (eRule[2] <= e && e < eRule[3])
            {
                eFuzzy[0] = (eRule[3] - e) / (eRule[3] - eRule[2]);
                pe = 2;
            }
            else if (eRule[3] <= e && e < eRule[4])
            {
                eFuzzy[0] = (eRule[4] - e) / (eRule[4] - eRule[3]);
                pe = 3;
            }
            else if (eRule[4] <= e && e < eRule[5])
            {
                eFuzzy[0] = (eRule[5] - e) / (eRule[5] - eRule[4]);
                pe = 4;
            }
            else if (eRule[5] <= e && e < eRule[6])
            {
                eFuzzy[0] = (eRule[6] - e) / (eRule[6] - eRule[5]);
                pe = 5;
            }
            else
            {
                eFuzzy[0] = 0.0;
                pe = 5;
            }
            eFuzzy[1] = 1.0 - eFuzzy[0];
            /*****误差变化隶属函数描述*****/
            if (ec < ecRule[0])
            {
                ecFuzzy[0] = 1.0;
                pec = 0;
            }
            else if (ecRule[0] <= ec && ec < ecRule[1])
            {
                ecFuzzy[0] = (ecRule[1] - ec) / (ecRule[1] - ecRule[0]);
                pec = 0;
            }
            else if (ecRule[1] <= ec && ec < ecRule[2])
            {
                ecFuzzy[0] = (ecRule[2] - ec) / (ecRule[2] - ecRule[1]);
                pec = 1;
            }
            else if (ecRule[2] <= ec && ec < ecRule[3])
            {
                ecFuzzy[0] = (ecRule[3] - ec) / (ecRule[3] - ecRule[2]);
                pec = 2;
            }
            else if (ecRule[3] <= ec && ec < ecRule[4])
            {
                ecFuzzy[0] = (ecRule[4] - ec) / (ecRule[4] - ecRule[3]);
                pec = 3;
            }
            else if (ecRule[4] <= ec && ec < ecRule[5])
            {
                ecFuzzy[0] = (ecRule[5] - ec) / (ecRule[5] - ecRule[4]);
                pec = 4;
            }
            else if (ecRule[5] <= ec && ec < ecRule[6])
            {
                ecFuzzy[0] = (ecRule[6] - ec) / (ecRule[6] - ecRule[5]);
                pec = 5;
            }
            else
            {
                ecFuzzy[0] = 0.0;
                pec = 5;
            }
            ecFuzzy[1] = 1.0 - ecFuzzy[0];
            /***********查询模糊规则表***************/
            num = KiRule[pe, pec];
            KiFuzzy[num] += eFuzzy[0] * ecFuzzy[0];
            num = KiRule[pe, pec + 1];
            KiFuzzy[num] += eFuzzy[0] * ecFuzzy[1];
            num = KiRule[pe + 1, pec];
            KiFuzzy[num] += eFuzzy[1] * ecFuzzy[0];
            num = KiRule[pe + 1, pec + 1];
            KiFuzzy[num] += eFuzzy[1] * ecFuzzy[1];
            /********加权平均法解模糊********/
            Ki_calcu = KiFuzzy[0] * kiRule[0] + KiFuzzy[1] * kiRule[1] + KiFuzzy[2] * kiRule[2]
+ KiFuzzy[3] * kiRule[3];
            return (Ki_calcu);
        }

        /***********************************************
         函数功能：PID参数Kd的计算 
************************************************/
        double fuzzy_kd(double e, double ec)
        {
            double Kd_calcu;
            int num, pe, pec;
            double[] eRule = { -3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0 };
            double[] ecRule = { -3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0 };
            double[] eFuzzy = { 0.0, 0.0 };
            double[] ecFuzzy = { 0.0, 0.0 };
            double[] kdRule = { 0.0, 1.0, 2.0, 3.0 };
            double[] KdFuzzy = { 0.0, 0.0, 0.0, 0.0 };
            int[,] KdRule =
                        {
                            { 3,3,3,2,2,2,2 },
                            { 2,2,2,1,1,1,1 },
                            { 1,1,2,1,1,2,1 },
                            { 1,1,0,1,0,1,1 },
                            { 1,1,0,0,0,1,1 },
                            { 2,2,1,0,1,1,1 },
                            { 3,3,3,3,2,3,2 }
                        };
            /*****误差隶属函数描述*****/
            if (e < eRule[0])
            {
                eFuzzy[0] = 1.0;
                pe = 0;
            }
            else if (eRule[0] <= e && e < eRule[1])
            {
                eFuzzy[0] = (eRule[1] - e) / (eRule[1] - eRule[0]);
                pe = 0;
            }
            else if (eRule[1] <= e && e < eRule[2])
            {
                eFuzzy[0] = (eRule[2] - e) / (eRule[2] - eRule[1]);
                pe = 1;
            }
            else if (eRule[2] <= e && e < eRule[3])
            {
                eFuzzy[0] = (eRule[3] - e) / (eRule[3] - eRule[2]);
                pe = 2;
            }
            else if (eRule[3] <= e && e < eRule[4])
            {
                eFuzzy[0] = (eRule[4] - e) / (eRule[4] - eRule[3]);
                pe = 3;
            }
            else if (eRule[4] <= e && e < eRule[5])
            {
                eFuzzy[0] = (eRule[5] - e) / (eRule[5] - eRule[4]);
                pe = 4;
            }
            else if (eRule[5] <= e && e < eRule[6])
            {
                eFuzzy[0] = (eRule[6] - e) / (eRule[6] - eRule[5]);
                pe = 5;
            }
            else
            {
                eFuzzy[0] = 0.0;
                pe = 5;
            }
            eFuzzy[1] = 1.0 - eFuzzy[0];

            /*****误差变化隶属函数描述*****/
            if (ec < ecRule[0])
            {
                ecFuzzy[0] = 1.0;
                pec = 0;
            }
            else if (ecRule[0] <= ec && ec < ecRule[1])
            {
                ecFuzzy[0] = (ecRule[1] - ec) / (ecRule[1] - ecRule[0]);
                pec = 0;
            }
            else if (ecRule[1] <= ec && ec < ecRule[2])
            {
                ecFuzzy[0] = (ecRule[2] - ec) / (ecRule[2] - ecRule[1]);
                pec = 1;
            }
            else if (ecRule[2] <= ec && ec < ecRule[3])
            {
                ecFuzzy[0] = (ecRule[3] - ec) / (ecRule[3] - ecRule[2]);
                pec = 2;
            }
            else if (ecRule[3] <= ec && ec < ecRule[4])
            {
                ecFuzzy[0] = (ecRule[4] - ec) / (ecRule[4] - ecRule[3]);
                pec = 3;
            }
            else if (ecRule[4] <= ec && ec < ecRule[5])
            {
                ecFuzzy[0] = (ecRule[5] - ec) / (ecRule[5] - ecRule[4]);
                pec = 4;
            }
            else if (ecRule[5] <= ec && ec < ecRule[6])
            {
                ecFuzzy[0] = (ecRule[6] - ec) / (ecRule[6] - ecRule[5]);
                pec = 5;
            }
            else
            {
                ecFuzzy[0] = 0.0;
                pec = 5;
            }
            ecFuzzy[1] = 1.0 - ecFuzzy[0];
            /***********查询模糊规则表*************/
            num = KdRule[pe, pec];
            KdFuzzy[num] += eFuzzy[0] * ecFuzzy[0];
            num = KdRule[pe, pec + 1];
            KdFuzzy[num] += eFuzzy[0] * ecFuzzy[1];
            num = KdRule[pe + 1, pec];
            KdFuzzy[num] += eFuzzy[1] * ecFuzzy[0];
            num = KdRule[pe + 1, pec + 1];
            KdFuzzy[num] += eFuzzy[1] * ecFuzzy[1];
            /********加权平均法解模糊********/
            Kd_calcu = KdFuzzy[0] * kdRule[0] + KdFuzzy[1] * kdRule[1] + KdFuzzy[2] * kdRule[2]
    + KdFuzzy[3] * kdRule[3];
            return (Kd_calcu);
        }



        public void test()
        {

            for (int i = 0; i < 50; i++)
            {
                this.PID_Calculate();
                Kp = this.fuzzy_kp(Err / 5, D_Err);              //E量化因子5
                Ki = this.fuzzy_ki(Err / 5, D_Err);
                Kd = this.fuzzy_kd(Err / 5, D_Err);
                Console.WriteLine($"{S_temp}  {P_temp}  {Kp}  {Ki}  {Kd}");
                if (S_temp > P_temp)
                {
                    P_temp += U / 10;
                }
                else
                {
                    P_temp -= U / 10;
                }
                if (i > 20)
                {
                    S_temp = 30;
                }
            }
        }


    }
}