dcgdeng
級別: *
|
#include <REGX51.H> #include <intrins.h> #include <string.h> #include <stdio.h> typedef struct PID { double SetPoint; double Proportion; double Integral; double Derivative; double LastError; double PrevError; double SumError; } PID; double PIDCalc( PID *pp, double NextPoint ) { double dError, Error; Error = pp->SetPoint - NextPoint; pp->SumError += Error; 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) return 100.0; } void actuator(double rDelta) {} void man(void) { PID sPID; double rOut; double rIn; PIDInit ( &sPID ); sPID.Proportion = 0.5; sPID.Integral = 0.5; sPID.Derivative = 0.0; sPID.SetPoint = 0.3; for (;;) { rIn = adc (); rOut = PIDCalc ( &sPID,rIn ); actuator ( rOut ); } } |
---|---|
|