www.pudn.com > mputil.rar > PID_controller.cpp

////////////////////////////////////////////////////////
// PID_controller.c   author Bret Fortenberry
// created 3/16/04 for use of the MPLab at UCSD
// 
// PID controller for the head moter movements
//
//////////////////////////////////////////////////////// 

#include 
#include 
#include 
#include  
#include 

#include "PID_controller.h"

PID_controller::PID_controller() {
	m_curError = 0.0;
	m_pGain = 2.5; //releation of percentage to actual movement
	m_lambda = 0.4; //smothing effect
	m_iGain = 0.05;
	m_dGain = 3.0;
	m_iMax = 20; 
	m_iMin = -20;
}

PID_controller::~PID_controller() {}

double PID_controller::update() {

	double D;
	double P;
	double I;

	D = m_dGain*(m_prevError - m_curError);
	P = m_pGain * m_curError;
	I = (m_lambda * m_iGain) + m_iSum; 
	m_prevError = m_curError;
	
	m_iSum = (m_lambda * m_iSum) + (1-m_lambda)*m_curError;
	m_iSum = min(m_iSum, m_iMax);
	m_iSum = max(m_iSum, m_iMin); 
			
	return(P - D + I);

}

void PID_controller::setGains(double pGain, double iGain, double dGain) {
	m_pGain = pGain; m_iGain = iGain; m_dGain = dGain;
}

void PID_controller::setMax(float iMax, float iMin) {
	m_iMax = iMax; m_iMin = iMin;
}

void PID_controller::setLambda(double lambda) { 
	m_lambda = lambda; 
}