File content as of revision 0:dd5d4837292c:

/************************************************************************************
* @author：
* @date : 2019/3/25
* @fuction name：FUZZY_CONTROL
* @fuction description： 模糊自适应控制算法
*************************************************************************************/
#include "fuzzy.h"

fuzzy_control_t ankleFuzzy, kneeFuzzy;


//模糊集合
/*
#define NL   -3
#define NM   -2
#define NS   -1
#define ZE   0
#define PS   1
#define PM   2
#define PL   3
*/
const float FuzzyRuleKp[7][7]={
    /*                  EC                     */
    /*  -3,       -2,     -1,     0,     1,      2,     3 */
/*-3*/ 473.86, 473.86, 416.63, 416.63, 333.37, 250.00, 250.00,
/*-2*/ 473.86, 473.86, 416.63, 333.37, 333.37, 250.00, 250.00,
/*-1*/ 416.63, 416.63, 416.63, 416.63, 250.00, 166.64, 166.64,
/* 0*/ 416.63, 416.63, 416.63, 250.00, 333.37, 83.38,  83.38,
/* 1*/ 333.37, 333.37, 250.00, 166.64, 166.64, 83.38,  83.38,
/* 2*/ 333.37, 250.00, 166.64, 83.38,  83.38,  83.38,  26.15,
/* 3*/ 250.00, 250.00, 83.38,  83.38,  83.38,  26.15,  26.15,
};

const float FuzzyRuleKd[7][7]={
    /*                  EC                     */
    /*  -3,   -2,   -1,   0,     1,   2,    3 */
/*-3*/ 3.33, 1.67, 0.26, 0.26, 0.26, 0.83, 3.33,
/*-2*/ 3.33, 2.50, 0.26, 0.26, 0.26, 0.83, 3.33,
/*-1*/ 2.50, 1.67, 1.67, 0.83, 1.67, 1.67, 2.50,
/* 0*/ 2.50, 1.67, 4.17, 1.67, 1.67, 1.67, 2.50,
/* 1*/ 2.50, 2.50, 2.50, 2.50, 2.50, 2.50, 2.50,
/* 2*/ 4.74, 1.67, 3.33, 3.33, 3.33, 3.33, 4.74,
/* 3*/ 4.74, 4.17, 4.17, 4.17, 3.33, 3.33, 0.26,
};


void fuzzy_init(fuzzy_control_t *fuzzy ,float max_e,float max_ec,float gain)
{
    fuzzy->quantied_e = 3.0f/max_e;
    fuzzy->quantied_ec = 3.0f/max_ec;
    fuzzy->quantied_resault = gain;
    fuzzy->outKp = 0.0f;
    fuzzy->outKd = 0.0f;
}
void fuzzy_control(float e,float ec,fuzzy_control_t *fuzzy,const float fuzzyRuleKp[7][7], const float fuzzyRuleKd[7][7])
{

     float etemp,ectemp;
     float eLefttemp,ecLefttemp;
     float eRighttemp ,ecRighttemp;

     int eLeftIndex,ecLeftIndex;
     int eRightIndex,ecRightIndex;
     e = e * fuzzy->quantied_e;
     ec = ec * fuzzy->quantied_ec;
     e = range(e,-3.0f,3.0f);
     ec = range(ec,-3.0f,3.0f);
     etemp = e > 3.0f ? 0.0f : (e < - 3.0f ? 0.0f : (e >= 0.0f ? (e >= 2.0f ? 2.5f: (e >= 1.0f ? 1.5f : 0.5f)) : (e >= -1.0f ? -0.5f : (e >= -2.0f ? -1.5f : (e >= -3.0f ? -2.5f : 0.0f)))));
     eLeftIndex = (int)((etemp-0.5f) + 3.0f);        //[-3,3] -> [0,6]
     eRightIndex = (int)((etemp+0.5f) + 3.0f);
     eLefttemp =etemp == 0.0f ? 0.0f:((etemp+0.5f)-e);             
     eRighttemp=etemp == 0.0f ? 0.0f:( e-(etemp-0.5f));
     
     ectemp = ec > 3.0f ? 0.0f : (ec < - 3.0f ? 0.0f : (ec >= 0.0f ? (ec >= 2.0f ? 2.5f: (ec >= 1.0f ? 1.5f : 0.5f)) : (ec >= -1.0f ? -0.5f : (ec >= -2.0f ? -1.5f : (ec >= -3.0f ? -2.5f : 0.0f) ))));
     ecLeftIndex = (int)((ectemp-0.5f) + 3.0f);        //[-6,6] -> [0,12]
     ecRightIndex = (int)((ectemp+0.5f) + 3.0f);
     ecLefttemp =ectemp == 0.0f ? 0.0f:((ectemp+0.5f)-ec);
     ecRighttemp=ectemp == 0.0f ? 0.0f:( ec-(ectemp-0.5f));


/*************************************反模糊*************************************/




    fuzzy->outKp = (eLefttemp * ecLefttemp * fuzzyRuleKp[eLeftIndex][ecLeftIndex]
                    + eLefttemp * ecRighttemp * fuzzyRuleKp[eLeftIndex][ecRightIndex]
                    + eRighttemp * ecLefttemp * fuzzyRuleKp[eRightIndex][ecLeftIndex]
                    + eRighttemp * ecRighttemp * fuzzyRuleKp[eRightIndex][ecRightIndex]);
                    
    fuzzy->outKd = (eLefttemp * ecLefttemp * fuzzyRuleKd[eLeftIndex][ecLeftIndex]
                    + eLefttemp * ecRighttemp * fuzzyRuleKd[eLeftIndex][ecRightIndex]
                    + eRighttemp * ecLefttemp * fuzzyRuleKd[eRightIndex][ecLeftIndex]
                    + eRighttemp * ecRighttemp * fuzzyRuleKd[eRightIndex][ecRightIndex]);

    //对解算出的模糊结果进行量化
    fuzzy->outKp = fuzzy->outKp * fuzzy->quantied_resault;
    fuzzy->outKd = fuzzy->outKd * fuzzy->quantied_resault;
}