// ClassicPID example code

// Define signal gains and limits
struct  heuristic  {
  float        lowlimit;        // low limit, membership 0.0
  float        highlimit;       // high limit, membership 1.0
  float        gain;            // weighting factor for inference combination
} ;
typedef struct heuristic  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

// PID gain factors for control strategy
#define  P_GAIN        10.000
#define  I_GAIN         0.040
#define  D_GAIN         0.120

static rule   P_heuristic = { -P_LIMIT, P_LIMIT, P_GAIN };
static rule   I_heuristic = { -I_LIMIT, I_LIMIT, I_GAIN };
static rule   D_heuristic = { -D_LIMIT, D_LIMIT, D_GAIN };
static rule   O_heuristic = { -O_LIMIT, O_LIMIT, 1.0 };

// Evaluate PID heuristic.  Accumulates results with previous results.
void   evaluate_heuristic(
    float variable, rule *pRule, float *output )
{
    float  in_value;
    if       ( variable >= pRule->highlimit )
        in_value = pRule->highlimit;
    else if  ( variable <= pRule->lowlimit )
        in_value = pRule->lowlimit;
    *output += in_value * pRule->gain;
}

float  output_limit( rule *pRule, float rawoutput )
{
    if  (rawoutput >= pRule->highlimit)
        return  pRule->highlimit;
    if  (rawoutput <= pRule->lowlimit)
        return  pRule->lowlimit;
    else return rawoutput;
}

float   compute_classic_PID ( float  Err, float  AccumErr, float  Speed )
{
    float  rawoutput = 0.0;
    evaluate_heuristic( Err,      &P_heuristic, &rawoutput );
    evaluate_heuristic( AccumErr, &I_heuristic, &rawoutput );
    evaluate_heuristic( Speed,    &D_heuristic, &rawoutput );
    return   output_limit( &O_heuristic, rawoutput );
}