#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 );            

    }
}