///<inheritdoc/>
public void Disable()
{
lock (m_lockObject)
{
PIDOutput.PidWrite(0);
m_enabled = false;
}
}
/// <summary>
/// Read the inpout, calculate the output accordinglyu, and write to the input.
/// This should only be called by the Notifier timer and is created during initialization.
/// </summary>
protected virtual void Calculate()
{
bool enabled;
IPIDSource pidInput;
lock (m_lockObject)
{
if (PIDInput == null)
{
return;
}
if (PIDOutput == null)
{
return;
}
enabled = m_enabled; // take snapshot of these values...
pidInput = PIDInput;
}
if (!enabled)
{
return;
}
double input;
double result;
IPIDOutput pidOutput = null;
lock (this)
{
input = pidInput.PidGet();
}
lock (m_lockObject)
{
m_error = m_setpoint - input;
if (m_continuous)
{
if (Math.Abs(m_error) > (m_maximumInput - m_minimumInput) / 2)
{
if (m_error > 0)
{
m_error = m_error - m_maximumInput + m_minimumInput;
}
else
{
m_error = m_error + m_maximumInput - m_minimumInput;
}
}
}
if (pidInput.PIDSourceType == PIDSourceType.Rate)
{
if (m_P != 0)
{
double potentialPGain = (m_totalError + m_error) * m_P;
if (potentialPGain < m_maximumOutput)
{
if (potentialPGain > m_minimumOutput)
{
m_totalError += m_error;
}
else
{
m_totalError = m_minimumOutput / m_P;
}
}
else
{
m_totalError = m_maximumOutput / m_P;
}
}
m_result = m_P * m_totalError + m_D * m_error + CalculateFeedForward();
}
else
{
if (m_I != 0.0)
{
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput)
{
if (potentialIGain > m_minimumInput)
{
m_totalError += m_error;
}
else
{
m_totalError = m_minimumOutput / m_I;
}
}
else
{
m_totalError = m_maximumOutput / m_I;
}
}
m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + CalculateFeedForward();
}
m_prevError = m_error;
if (m_result > m_maximumOutput)
{
m_result = m_maximumOutput;
}
else if (m_result < m_minimumOutput)
{
m_result = m_minimumOutput;
}
pidOutput = PIDOutput;
result = m_result;
m_buf.Enqueue(m_error);
m_bufTotal += m_error;
//Remove old elements when the buffer is full
if (m_buf.Count > m_bufLength)
{
m_bufTotal -= m_buf.Dequeue();
}
}
pidOutput.PidWrite(result);
}