typedef struct PID
{
double SetPoint; // Desired Value
double Proportion; // Proportional Const
double Integral; // Integral Const
double Derivative; // Derivative Const
double LastError; // Error[-1]
double PrevError; // Error[-2]
double SumError; // Sums of Errors
} PID;
double PIDCalc ( PID *pp, double NextPoint )
{ double dError, Error;
pp->SumError += (Error = pp->SetPoint - NextPoint);
dError = pp->LastError - pp->PrevError;
pp->PrevError = pp->LastError;
pp->LastError = Error;
return ( pp->Proportion * Error
+ pp->Integral * pp->SumError
+ pp->Derivative * dError
);
}
void PIDInit ( PID *pp )
{ memset ( pp,0,sizeof(PID) );
}
double sensor ( void ) // Dummy Sensor Function
{