typedef struct
{
  double dState;      // Last position input
  double iState;      // Integrator state
  double iMax, iMin;  // Maximum and minimum allowable integrator state
  double iGain, 	  // integral gain
       	 pGain, 	  // proportional gain
         dGain;    	  // derivative gain
} SPid;


double UpdatePID(SPid * pid, double error, double position)
{
	double pTerm,
	dTerm, iTerm;
	pTerm = pid->pGain * error;
  
	// calculate the proportional term
	// calculate the integral state with appropriate limiting
  
	pid->iState += error;
	
    if (pid->iState > pid->iMax) 
		pid->iState = pid->iMax;

  	else if (pid->iState < pid->iMin) 
		pid->iState = pid->iMin;

	iTerm = pid->iGain * iState; // calculate the integral term
	dTerm = pid->dGain * (position - pid->dState);

	pid->dState = position;
  
	return pTerm + iTerm - dTerm;
}