智能车模糊控制

#include “include.h“

/******此处将P改为中线的err ，D改为中线的斜率即可*********/

float Fuzzy（float Pfloat D）
{
/*输入量P语言值特征点*/ 
float PFF[7]={-15-10-5051015};
/*输入量D语言值特征点*/ 
float DFF[5]={-13-60613};
/*输出量U语言值特征点（根据赛道类型选择不同的输出值）*/
float UFF[7]={01520355080120};

int rule[7][5]={
//  -2  -1   0   1   2 ec    e
  { 1  1  1  0  0} //  -3
  { 4  2  1  0  0} //  -2
  { 6  4  1  1  0} //  -1
  { 3  3  0  3  3} //   0
  { 0  1  1  4  6} //   1
  { 0  0  1  2  4} //   2
  { 0  0  1  1  1}  //   3
};


float U=0;  /*偏差偏差微分以及输出值的精确量*/ 
float PF[2]={0}DF[2]={0}UF[4]={0};
 /*偏差偏差微分以及输出值的隶属度*/ 
int Pn=0Dn=0Un[4]={0};
float t1=0t2=0t3=0t4=0temp1=0temp2=0;
/*隶属度的确定*/ 
/*根据PD的指定语言值获得有效隶属度*/ 
 if（P>PFF[0] && P{
 if（P<=PFF[1]）
 {
  Pn=-2;
  PF[0]=（PFF[1]-P）/（PFF[1]-PFF[0]）;
 }
 else if（P<=PFF[2]）
 {
  Pn=-1;
  PF[0]=（PFF[2]-P）/（PFF[2]-PFF[1]）;
 }
 else if（P<=PFF[3]）
 {
  Pn=0;
  PF[0]=（PFF[3]-P）/（PFF[3]-PFF[2]）;
 }
 else if（P<=PFF[4]）
 {
  Pn=1;
  PF[0]=（PFF[4]-P）/（PFF[4]-PFF[3]）;
 }
 else if（P<=PFF[5]）
 {
  Pn=2;
  PF[0]=（PFF[5]-P）/（PFF[5]-PFF[4]）;
 }
 else if（P<=PFF[6]）
 {
  Pn=3;
  PF[0]=（PFF[6]-P）/（PFF[6]-PFF[5]）;
 }
}
 
 else if（P<=PFF[0]）
 {
  Pn=-2;
  PF[0]=1;
 }
 else if（P>=PFF[6]）
 {
  Pn=3;
  PF[0]=0;
 }

PF[1]=1-PF[0];


//判断D的隶属度
 if（D>DFF[0]&&D {
  if（D<=DFF[1]）
  {
   Dn=-2;
   DF[0]=（DFF[1]-D）/（DFF[1]-DFF[0]）;
  }