模糊控制程序开源,模糊PID C语言实现及调模糊规则表方法

[复制链接]
查看2 | 回复0 | 2021-4-21 15:20:59 | 显示全部楼层 |阅读模式
模糊控制代码,只需要给出偏差量,偏差范围,偏差变化率范围和你想要控制的量的范围(比如P参数的变化范围),再有一个合适的模糊规则就可使用,无需关心引擎内部逻辑,安安心心的调表就行。
关于如何调表:拿智能车比赛举例,使用模糊控制后发现某一段线路跑的不好(即某一条模糊规则不理想),读取此时的e_cache与ec_cache就能定位到模糊规则表的位置,修改该位置的表就行,注意,修改一处位置(可能为1个量,2个量或者4个量)后会导致周围的规则变化,即发现其他线路跑法变了,毕竟控制本身就是个鱼与熊掌不可兼得的事嘛。
源程序如下:
  1. /*******!!! 该宏定义不可修改 !!!******/
  2. #define NB 1
  3. #define NM 2
  4. #define NS 3
  5. #define ZO 4
  6. #define PS 5
  7. #define PM 6
  8. #define PB 7
  9. /*** 不可修改内容结束 ****/
  10. //N为negative,P为positive,B为big,M为middle,S为small,ZO为zero

  11. int8 DirectionError[2][3]={0};//-128~127,这里储存偏差量,注意数字范围
  12. #define E_MAX 120    //偏差E的最大值(预设),程序会自动分割成正负对称数列
  13. #define EC_MAX 60    //偏差变化EC的最大值(预设),程序会自动分割成正负对称数列
  14. #define P_MAX  10    //方向环P参数的最大值(预设),若不需要负数列,请修改下面的自动分割逻辑
  15. #define D_MAX  10    //方向环D参数的最大值(预设),若不需要负数列,请修改下面的自动分割逻辑
  16. int8 E_NB,E_NM,E_NS,E_ZO,E_PS,E_PM,E_PB;                      //E的界限值
  17. float E_NB_D,E_NM_D,E_NS_D,E_ZO_D,E_PS_D,E_PM_D,E_PB_D;       //用于保存E的隶属度
  18. int8 EC_NB,EC_NM,EC_NS,EC_ZO,EC_PS,EC_PM,EC_PB;               //EC的界限值
  19. float EC_NB_D,EC_NM_D,EC_NS_D,EC_ZO_D,EC_PS_D,EC_PM_D,EC_PB_D;//用于保存EC的隶属度
  20. float E_EC_NB_D,E_EC_NM_D,E_EC_NS_D,E_EC_ZO_D,E_EC_PS_D,E_EC_PM_D,E_EC_PB_D;//用于保存E_EC的隶属度
  21. float P_NB,P_NM,P_NS,P_ZO,P_PS,P_PM,P_PB;                     //输出P的界限值
  22. float P_NB_D,P_NM_D,P_NS_D,P_ZO_D,P_PS_D,P_PM_D,P_PB_D;       //用于保存P的隶属度
  23. float D_NB,D_NM,D_NS,D_ZO,D_PS,D_PM,D_PB;                     //输出D的界限值
  24. float D_NB_D,D_NM_D,D_NS_D,D_ZO_D,D_PS_D,D_PM_D,D_PB_D;       //用于保存D的隶属度
  25. float P_out,D_out;                                            //用于保存最后的P,D输出

  26. int16 Direction_PID;                  //用于保存方向PID的输出(直接加在速度环的输入上,此时速度环必需指定基准速度)

  27. float Fuzzy_a,Fuzzy_b,Fuzzy_c,Fuzzy_d;
  28. void Fuzzy_Preprocess(void)//用于自动处理预设数据
  29. {
  30.         //自动等分,只需运行一次
  31.         Fuzzy_a=E_MAX/4;
  32.         Fuzzy_b=EC_MAX/4;
  33.         Fuzzy_c=P_MAX/4;
  34.         Fuzzy_d=D_MAX/4;
  35.         E_NB = -E_MAX+Fuzzy_a,E_NM = -E_MAX+Fuzzy_a*2,E_NS = -E_MAX+Fuzzy_a*3,E_ZO=E_MAX-Fuzzy_a*4,E_PS = E_MAX-Fuzzy_a*3,E_PM = E_MAX-Fuzzy_a*2,E_PB = E_MAX-Fuzzy_a;
  36.         EC_NB = -EC_MAX+Fuzzy_b,EC_NM = -EC_MAX+Fuzzy_b*2,EC_NS = -EC_MAX+Fuzzy_b*3,EC_ZO=EC_MAX-Fuzzy_b*4,EC_PS = EC_MAX-Fuzzy_b*3,EC_PM = EC_MAX-Fuzzy_b*2,EC_PB = EC_MAX-Fuzzy_b;
  37.         P_NB = -P_MAX+Fuzzy_c,P_NM = -P_MAX+Fuzzy_c*2,P_NS = -P_MAX+Fuzzy_c*3,P_ZO=P_MAX-Fuzzy_c*4,P_PS = P_MAX-Fuzzy_c*3,P_PM = P_MAX-Fuzzy_c*2,P_PB = P_MAX-Fuzzy_c;
  38.         D_NB = -D_MAX+Fuzzy_d,D_NM = -D_MAX+Fuzzy_d*2,D_NS = -D_MAX+Fuzzy_d*3,D_ZO=D_MAX-Fuzzy_d*4,D_PS = D_MAX-Fuzzy_d*3,D_PM = D_MAX-Fuzzy_d*2,D_PB = D_MAX-Fuzzy_d;
  39.         //用于保存预处理好的范围数据
  40.         int8 E_scope[9]={-E_MAX,E_NB,E_NM,E_NS,E_ZO,E_PS,E_PM,E_PB,E_MAX};//误差范围
  41.         int8 EC_scope[9]={-EC_MAX,EC_NB,EC_NM,EC_NS,EC_ZO,EC_PS,EC_PM,EC_PB,EC_MAX};//误差变化范围
  42.         int8 P_scope[9]={-P_MAX,P_NB,P_NM,P_NS,P_ZO,P_PS,P_PM,P_PB,P_MAX};//P范围
  43.         int8 D_scope[9]={-D_MAX,D_NB,D_NM,D_NS,D_ZO,D_PS,D_PM,D_PB,D_MAX};//D范围
  44. }
  45. //用于保存模糊规则    EC  NB NM NS ZO PS PM PB
  46. int8 Fuzzy_Rule[7*4][7]={{PB,PB,PB,PB,PB,PB,PB},//模糊控制表
  47.                                                  {PB,PB,PB,PB,PM,PM,PS},//左边为模糊规则,右边保存预处理的输出隶属度
  48.                                                  {PM,PM,PM,PS,PS,ZO,ZO},//该表用于直接控制,即输出为实际物理量
  49.                                                  {PM,PS,PS,ZO,NS,NM,NM},//该表可根据需要修改
  50.                                                  {ZO,NS,NS,NM,NM,NM,NB},
  51.                                                  {NS,NS,NM,NM,NM,NB,NB},
  52.                                                  {NM,NM,NB,NB,NB,NB,NB},
  53.                                         /*E*/
  54.                                    /*NB*/{PB,PB,PM,PM,PS,ZO,ZO},//模糊P规则表
  55.                                    /*NM*/{PB,PB,PM,PS,PS,ZO,NS},//左边为模糊规则,右边保存预处理的输出隶属度
  56.                                    /*NS*/{PM,PM,PM,PS,ZO,NS,NS},//该表用于控制PID的参数P,即输出为P值或P值的变化量
  57.                                    /*ZO*/{PM,PM,PS,ZO,NS,NM,NM},//该表必须修改,不可直接使用
  58.                                    /*PS*/{PS,PS,ZO,NS,NS,NM,NM},
  59.                                    /*PM*/{PS,ZO,PS,NM,NM,NM,NB},
  60.                                    /*PB*/{ZO,ZO,NM,NM,NM,NB,NB},

  61.                                                  {NB,NB,NM,NM,NS,ZO,ZO},//模糊I规则表
  62.                                                  {NB,NB,NM,NS,NS,ZO,ZO},//左边为模糊规则,右边保存预处理的输出隶属度
  63.                                                  {NB,NM,NS,NS,ZO,PS,PS},//该表用于控制PID的参数I,即输出为I值或I值的变化量
  64.                                                  {NM,NM,NS,ZO,PS,PM,PM},//该表必须修改,不可直接使用
  65.                                                  {NM,NS,ZO,PS,PS,PM,PB},
  66.                                                  {ZO,ZO,PS,PS,PM,PB,PB},
  67.                                                  {ZO,ZO,PS,PM,PM,PB,PB},

  68.                                                  {NB,NB,NB,NB,NB,NM,PS},//模糊D规则表
  69.                                                  {PS,NS,NB,NM,NM,NS,ZO},//左边为模糊规则,右边保存预处理的输出隶属度
  70.                                                  {ZO,NS,NM,NM,NS,NS,ZO},//该表用于控制PID的参数D,即输出为D值或D值的变化量
  71.                                                  {ZO,NS,NS,NS,NS,NS,ZO},//该表必须修改,不可直接使用
  72.                                                  {ZO,ZO,ZO,ZO,ZO,ZO,ZO},
  73.                                                  {PB,ZO,PS,PS,PS,PS,PB},
  74.                                                  {PB,PM,PM,PM,PS,PS,PB},
  75. };

  76. //用于指引模糊判断,临时变量
  77. int8 e_cache=0;
  78. int8 ec_cache=0;
  79. void Fuzzy_Menbership(int E)//计算输入隶属度
  80. {
  81.         //记录偏差
  82.         int EC;
  83.         EC=E-DirectionError[0][2];
  84.         for(uint8 i=0;i<2;i++)
  85.         {
  86.                 DirectionError[0][i]=DirectionError[0][i+1];//数据左移,低位扔掉
  87.         }
  88.         DirectionError[0][2]=E;
  89.         
  90.         //计算模糊输入隶属度
  91.         if(E <= E_NB)            E_NB_D=1,e_cache=0;
  92.         if(E > E_NB && E < E_NM) E_NB_D=(E_NM-E)/Fuzzy_a,E_NM_D=(E-E_NB)/Fuzzy_a,e_cache=1;
  93.         if(E > E_NM && E < E_NS) E_NM_D=(E_NS-E)/Fuzzy_a,E_NS_D=(E-E_NM)/Fuzzy_a,e_cache=2;
  94.         if(E > E_NS && E < E_ZO) E_NS_D=(E_ZO-E)/Fuzzy_a,E_ZO_D=(E-E_NS)/Fuzzy_a,e_cache=3;
  95.         if(E > E_ZO && E < E_PS) E_ZO_D=(E_PS-E)/Fuzzy_a,E_PS_D=(E-E_ZO)/Fuzzy_a,e_cache=4;
  96.         if(E > E_PS && E < E_PM) E_PS_D=(E_PM-E)/Fuzzy_a,E_PM_D=(E-E_PS)/Fuzzy_a,e_cache=5;
  97.         if(E > E_PM && E < E_PB) E_PM_D=(E_PB-E)/Fuzzy_a,E_PB_D=(E-E_PM)/Fuzzy_a,e_cache=6;
  98.         if(E >= E_PB)            E_PB_D=1,e_cache=7;
  99.         
  100.         if(EC <= EC_NB)              EC_NB_D=1,ec_cache=0;
  101.         if(EC > EC_NB && EC < EC_NM) EC_NB_D=(EC_NM-EC)/Fuzzy_b,EC_NM_D=(EC-EC_NB)/Fuzzy_b,ec_cache=1;
  102.         if(EC > EC_NM && EC < EC_NS) EC_NM_D=(EC_NS-EC)/Fuzzy_b,EC_NS_D=(EC-EC_NM)/Fuzzy_b,ec_cache=2;
  103.         if(EC > EC_NS && EC < EC_ZO) EC_NS_D=(EC_ZO-EC)/Fuzzy_b,EC_ZO_D=(EC-EC_NS)/Fuzzy_b,ec_cache=3;
  104.         if(EC > EC_ZO && EC < EC_PS) EC_ZO_D=(EC_PS-EC)/Fuzzy_b,EC_PS_D=(EC-EC_ZO)/Fuzzy_b,ec_cache=4;
  105.         if(EC > EC_PS && EC < EC_PM) EC_PS_D=(EC_PM-EC)/Fuzzy_b,EC_PM_D=(EC-EC_PS)/Fuzzy_b,ec_cache=5;
  106.         if(EC > EC_PM && EC < EC_PB) EC_PM_D=(EC_PB-EC)/Fuzzy_b,EC_PB_D=(EC-EC_PM)/Fuzzy_b,ec_cache=6;
  107.         if(EC >= EC_PB)              EC_PB_D=1,ec_cache=7;
  108. }

  109. //用于保存经过预处理的输出隶属度
  110. float Fuzzy_MP[7*4][8]={0};
  111. //读取规则并写入融合隶属度
  112. void Fuzzy_RD(int a,int b)//a:数据存储模式 b:规则切换
  113. {
  114.         switch (a)
  115.         {
  116.                 case 1:switch(Fuzzy_Rule[e_cache][ec_cache])
  117.                 {
  118.                         case 1:E_EC_NB_D=E_EC_NB_D+Fuzzy_MP[e_cache+7*b][ec_cache];break;
  119.                         case 2:E_EC_NM_D=E_EC_NM_D+Fuzzy_MP[e_cache+7*b][ec_cache];break;
  120.                         case 3:E_EC_NS_D=E_EC_NS_D+Fuzzy_MP[e_cache+7*b][ec_cache];break;
  121.                         case 4:E_EC_ZO_D=E_EC_ZO_D+Fuzzy_MP[e_cache+7*b][ec_cache];break;
  122.                         case 5:E_EC_PS_D=E_EC_PS_D+Fuzzy_MP[e_cache+7*b][ec_cache];break;
  123.                         case 6:E_EC_PM_D=E_EC_PM_D+Fuzzy_MP[e_cache+7*b][ec_cache];break;
  124.                         case 7:E_EC_PB_D=E_EC_PB_D+Fuzzy_MP[e_cache+7*b][ec_cache];break;
  125.                 };break;
  126.                 case 2:switch(Fuzzy_Rule[e_cache][ec_cache])
  127.                 {
  128.                         case 1:E_EC_NB_D=E_EC_NB_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break;
  129.                         case 2:E_EC_NM_D=E_EC_NM_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break;
  130.                         case 3:E_EC_NS_D=E_EC_NS_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break;
  131.                         case 4:E_EC_ZO_D=E_EC_ZO_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break;
  132.                         case 5:E_EC_PS_D=E_EC_PS_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break;
  133.                         case 6:E_EC_PM_D=E_EC_PM_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break;
  134.                         case 7:E_EC_PB_D=E_EC_PB_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break;
  135.                 };break;
  136.                 case 3:switch(Fuzzy_Rule[e_cache][ec_cache])
  137.                 {
  138.                         case 1:E_EC_NB_D=E_EC_NB_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break;
  139.                         case 2:E_EC_NM_D=E_EC_NM_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break;
  140.                         case 3:E_EC_NS_D=E_EC_NS_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break;
  141.                         case 4:E_EC_ZO_D=E_EC_ZO_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break;
  142.                         case 5:E_EC_PS_D=E_EC_PS_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break;
  143.                         case 6:E_EC_PM_D=E_EC_PM_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break;
  144.                         case 7:E_EC_PB_D=E_EC_PB_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break;
  145.                 };break;
  146.                 case 4:switch(Fuzzy_Rule[e_cache][ec_cache])
  147.                 {
  148.                         case 1:E_EC_NB_D=E_EC_NB_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break;
  149.                         case 2:E_EC_NM_D=E_EC_NM_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break;
  150.                         case 3:E_EC_NS_D=E_EC_NS_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break;
  151.                         case 4:E_EC_ZO_D=E_EC_ZO_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break;
  152.                         case 5:E_EC_PS_D=E_EC_PS_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break;
  153.                         case 6:E_EC_PM_D=E_EC_PM_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break;
  154.                         case 7:E_EC_PB_D=E_EC_PB_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break;
  155.                 };break;
  156.         }        
  157. }

  158. //模糊引擎
  159. float Fuzzy_Engine(int8 a)//0:模糊控制 1:模糊P 2:模糊I 3:模糊D
  160. {
  161.         E_EC_NB_D=E_EC_NM_D=E_EC_NS_D=E_EC_ZO_D=E_EC_PS_D=E_EC_PM_D=E_EC_PB_D=0;//模糊输出隶属度预处理(清零)
  162.         switch(e_cache)
  163.         {
  164.                 case 0:
  165.                                 {
  166.                                         switch (ec_cache)
  167.                                         {
  168.                                                 case 0:Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_NB_D,Fuzzy_RD(1,a);break;
  169.                                                 case 1:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_NM_D,Fuzzy_RD(1,a);break;
  170.                                                 case 2:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_NS_D,Fuzzy_RD(1,a);break;
  171.                                                 case 3:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_ZO_D,Fuzzy_RD(1,a);break;
  172.                                                 case 4:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_PS_D,Fuzzy_RD(1,a);break;
  173.                                                 case 5:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_PM_D,Fuzzy_RD(1,a);break;
  174.                                                 case 6:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_PB_D,Fuzzy_RD(1,a);break;
  175.                                                 case 7:Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_PB_D,Fuzzy_RD(1,a);break;
  176.                                         }
  177.                                 }break;
  178.                 case 1:
  179.                                 {
  180.                                         switch (ec_cache)
  181.                                         {
  182.                                                 case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_NB_D,Fuzzy_RD(1,a);break;
  183.                                                 case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_NM_D,Fuzzy_RD(1,a);break;
  184.                                                 case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_NS_D,Fuzzy_RD(1,a);break;
  185.                                                 case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_ZO_D,Fuzzy_RD(1,a);break;
  186.                                                 case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_PS_D,Fuzzy_RD(1,a);break;
  187.                                                 case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_PM_D,Fuzzy_RD(1,a);break;
  188.                                                 case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_PB_D,Fuzzy_RD(1,a);break;
  189.                                                 case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_PB_D,Fuzzy_RD(1,a);break;
  190.                                         }
  191.                                 }break;
  192.                 case 2:
  193.                                 {
  194.                                         switch (ec_cache)
  195.                                         {
  196.                                                 case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_NB_D,Fuzzy_RD(1,a);break;
  197.                                                 case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_NM_D,Fuzzy_RD(1,a);break;
  198.                                                 case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_NS_D,Fuzzy_RD(1,a);break;
  199.                                                 case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_ZO_D,Fuzzy_RD(1,a);break;
  200.                                                 case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_PS_D,Fuzzy_RD(1,a);break;
  201.                                                 case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_PM_D,Fuzzy_RD(1,a);break;
  202.                                                 case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_PB_D,Fuzzy_RD(1,a);break;
  203.                                                 case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_PB_D,Fuzzy_RD(1,a);break;
  204.                                         }
  205.                                 }break;
  206.                 case 3:
  207.                                 {
  208.                                         switch (ec_cache)
  209.                                         {
  210.                                                 case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_NB_D,Fuzzy_RD(1,a);break;
  211.                                                 case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_NM_D,Fuzzy_RD(1,a);break;
  212.                                                 case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_NS_D,Fuzzy_RD(1,a);break;
  213.                                                 case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_ZO_D,Fuzzy_RD(1,a);break;
  214.                                                 case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_PS_D,Fuzzy_RD(1,a);break;
  215.                                                 case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_PM_D,Fuzzy_RD(1,a);break;
  216.                                                 case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_PB_D,Fuzzy_RD(1,a);break;
  217.                                                 case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_PB_D,Fuzzy_RD(1,a);break;
  218.                                         }
  219.                                 }break;
  220.                 case 4:
  221.                                 {
  222.                                         switch (ec_cache)
  223.                                         {
  224.                                                 case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_NB_D,Fuzzy_RD(1,a);break;
  225.                                                 case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_NM_D,Fuzzy_RD(1,a);break;
  226.                                                 case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_NS_D,Fuzzy_RD(1,a);break;
  227.                                                 case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_ZO_D,Fuzzy_RD(1,a);break;
  228.                                                 case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_PS_D,Fuzzy_RD(1,a);break;
  229.                                                 case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_PM_D,Fuzzy_RD(1,a);break;
  230.                                                 case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_PB_D,Fuzzy_RD(1,a);break;
  231.                                                 case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_PB_D,Fuzzy_RD(1,a);break;
  232.                                         }
  233.                                 }break;
  234.                 case 5:
  235.                                 {
  236.                                         switch (ec_cache)
  237.                                         {
  238.                                                 case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_NB_D,Fuzzy_RD(1,a);break;
  239.                                                 case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_NM_D,Fuzzy_RD(1,a);break;
  240.                                                 case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_NS_D,Fuzzy_RD(1,a);break;
  241.                                                 case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_ZO_D,Fuzzy_RD(1,a);break;
  242.                                                 case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_PS_D,Fuzzy_RD(1,a);break;
  243.                                                 case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_PM_D,Fuzzy_RD(1,a);break;
  244.                                                 case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_PB_D,Fuzzy_RD(1,a);break;
  245.                                                 case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_PB_D,Fuzzy_RD(1,a);break;
  246.                                         }
  247.                                 }break;
  248.                 case 6:               
  249.                                 {
  250.                                         switch (ec_cache)
  251.                                         {
  252.                                                 case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NB_D,Fuzzy_RD(1,a);break;
  253.                                                 case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NM_D,Fuzzy_RD(1,a);break;
  254.                                                 case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NS_D,Fuzzy_RD(1,a);break;
  255.                                                 case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_ZO_D,Fuzzy_RD(1,a);break;
  256.                                                 case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PS_D,Fuzzy_RD(1,a);break;
  257.                                                 case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PM_D,Fuzzy_RD(1,a);break;
  258.                                                 case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PB_D,Fuzzy_RD(1,a);break;
  259.                                                 case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PB_D,Fuzzy_RD(1,a);break;
  260.                                         }
  261.                                 }break;
  262.                 case 7:        
  263.                                 {
  264.                                         switch (ec_cache)
  265.                                         {
  266.                                                 case 0:Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NB_D,Fuzzy_RD(1,a);break;
  267.                                                 case 1:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NM_D,Fuzzy_RD(1,a);break;
  268.                                                 case 2:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NS_D,Fuzzy_RD(1,a);break;
  269.                                                 case 3:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_ZO_D,Fuzzy_RD(1,a);break;
  270.                                                 case 4:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PS_D,Fuzzy_RD(1,a);break;
  271.                                                 case 5:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PM_D,Fuzzy_RD(1,a);break;
  272.                                                 case 6:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PB_D,Fuzzy_RD(1,a);break;
  273.                                                 case 7:Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PB_D,Fuzzy_RD(1,a);break;
  274.                                         }
  275.                                 }break;
  276.         }
  277. }

  278. int Fuzzy_PID(int E)//模糊调用函数示范
  279. {
  280.         Fuzzy_Preprocess();//用于自动处理预设数据,只需运行一次
  281.         Fuzzy_Menbership(E);//计算输入隶属度
  282.         Fuzzy_Engine(1);//计算模糊P
  283.         //重心法解模糊
  284.         //计算本次需调节的量,其中PD可直接用,也可加在预设的基准值上,这取决于PD的变化范围
  285.         P_out = E_EC_NB_D*P_NB + E_EC_NM_D*P_NM + E_EC_NS_D*P_NS + E_EC_ZO_D*P_ZO
  286.                   + E_EC_PS_D*P_PS + E_EC_PM_D*P_PM + E_EC_PB_D*P_PB;
  287.         Fuzzy_Engine(3);//计算模糊D
  288.         D_out = E_EC_NB_D*D_NB + E_EC_NM_D*D_NM + E_EC_NS_D*D_NS + E_EC_ZO_D*D_ZO
  289.                   + E_EC_PS_D*D_PS + E_EC_PM_D*D_PM + E_EC_PB_D*D_PB;
  290.         Direction_PID = P_out * (DirectionError[0][2] - DirectionError[0][1])
  291.                                   + D_out * (DirectionError[0][2] - 2*DirectionError[0][1] + DirectionError[0][0]);
  292.         return Direction_PID;
  293. }
复制代码


回复

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则