Picproje Elektronik Sitesi

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;
}

typedef struct

{
  double dState;      	                               
  
  double iState;      	                              
  
  double iMax, iMin;  	
                                                     
  
   double	iGain,    	                   
        	pGain,    	                   
         	dGain;     	                  

} SPid;
double UpdatePID(SPid * pid, double error, double position)

{
  double pTerm,iTerm,dTerm;
  
  pTerm = pid->pGain * error;                     
  

  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;                   
  dTerm = pid->dGain * (position - pid->dState);
  pid->dState = position;
  return pTerm + iTerm - dTerm;
}

error = spdRef - monitor.speed;
spdInteg += (DT * error * spdPid.ki);
spdInteg = limit(spdInteg, -95, 95);
pTerm = error * spdPid.kp;
iTerm = spdInteg;
dTerm = error * spdPid.kd / DT;
effort = limit(pTerm + iTerm + dTerm, -95, 95);
setDuty(effort);