// Fuzzy PID example code

// Define rule base
struct  rule_set  {
  float        lowlimit;        // low limit, membership 0.0
  float        highlimit;       // high limit, membership 1.0
  float        weighting;       // weighting factor for inference combination
} ;
typedef struct rule_set  rule;

// Membership bounds on input and output levels
#define  P_LIMIT          20000.0
#define  I_LIMIT         400000.0
#define  D_LIMIT           5000.0
#define  O_LIMIT          20000.0

// Weighting factors for inference
#define  P_RULE_WEIGHT    10.000
#define  I_RULE_WEIGHT     0.040
#define  D_RULE_WEIGHT     0.120

static rule   P_rule = { -P_LIMIT, P_LIMIT, P_RULE_WEIGHT };
static rule   I_rule = { -I_LIMIT, I_LIMIT, I_RULE_WEIGHT };
static rule   D_rule = { -D_LIMIT, D_LIMIT, D_RULE_WEIGHT };
static rule   O_rule = { -O_LIMIT, O_LIMIT, 1.0f };

// Evaluate "is member" and "is not member" of input value, given rule
// to apply.  Accumulates results with previous results. Takes advantage
// of special complementation properties of this rule set.
void   evaluate_support(
    float variable, rule *pRule, float *isMember, float *isNotMember )
{
    float  in_set;
    if       ( variable >= pRule->highlimit )
        in_set = 1.0;
    else if  ( variable <= pRule->lowlimit )
        in_set = 0.0;
    else
        in_set = (variable - pRule->lowlimit) /
                 (pRule->highlimit - pRule->lowlimit);
    *isMember    += in_set * pRule->weighting ;
    *isNotMember +=  (1.0 - in_set) * pRule->weighting ;
}

// Normalize and evaluate output
float  defuzzify( rule *pRule, float isMember, float isNotMember )
{
    float  output;
    output = (isMember * pRule->highlimit) + (isNotMember * pRule->lowlimit);
    if  (output>pRule->highlimit)  output=pRule->highlimit;
    if  (output>pRule->lowlimit)   output=pRule->lowlimit;
    return output;
}

float   compute_fuzzy_PID ( float  Err, float  AccumErr, float  Speed )
{
    float  isMember = 0.0;
    float  isNotMember = 0.0;
    evaluate_support( Err,      &P_rule, &isMember, &isNotMember );
    evaluate_support( AccumErr, &I_rule, &isMember, &isNotMember );
    evaluate_support( Speed,    &D_rule, &isMember, &isNotMember );
    return  defuzzify( &O_rule, isMember, isNotMember );
}