/**
* Read the input, calculate the output accordingly, and write to the output.
* This should only be called by the Notifier.
*/
void PIDController::Calculate() {
bool enabled;
PIDSource* pidInput;
PIDOutput* pidOutput;
{
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
pidInput = m_pidInput;
pidOutput = m_pidOutput;
enabled = m_enabled;
}
if (pidInput == nullptr) return;
if (pidOutput == nullptr) return;
if (enabled) {
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
float input = pidInput->PIDGet();
float result;
PIDOutput* pidOutput;
m_error = m_setpoint - input;
if (m_continuous) {
if (fabs(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 (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
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_D * m_error + m_P * m_totalError + CalculateFeedForward();
} else {
if (m_I != 0) {
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput) {
if (potentialIGain > m_minimumOutput)
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 = m_pidOutput;
result = m_result;
pidOutput->PIDWrite(result);
// Update the buffer.
m_buf.push(m_error);
m_bufTotal += m_error;
// Remove old elements when buffer is full.
if (m_buf.size() > m_bufLength) {
m_bufTotal -= m_buf.front();
m_buf.pop();
}
}
}
/**
* Read the input, calculate the output accordingly, and write to the output.
*
* This should only be called by the Notifier.
*/
void PIDController::Calculate() {
bool enabled;
PIDSource* pidInput;
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
if (m_pidInput == 0) return;
if (m_pidOutput == 0) return;
enabled = m_enabled;
pidInput = m_pidInput;
}
if (enabled) {
float input = pidInput->PIDGet();
float result;
PIDOutput* pidOutput;
{
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_error = m_setpoint - input;
if (m_continuous) {
if (fabs(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 (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
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_D * m_error + m_P * m_totalError + CalculateFeedForward();
} else {
if (m_I != 0) {
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput) {
if (potentialIGain > m_minimumOutput) {
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 = m_pidOutput;
result = m_result;
}
pidOutput->PIDWrite(result);
}
}
/**
* Read the input, calculate the output accordingly, and write to the output.
* This should only be called by the Notifier indirectly through CallCalculate
* and is created during initialization.
*/
void PIDController::Calculate() {
bool enabled;
PIDSource* pidInput;
float dt = m_loopTimer.Get();
CRITICAL_REGION(m_semaphore) {
if (m_pidInput == 0) return;
if (m_pidOutput == 0) return;
enabled = m_enabled;
pidInput = m_pidInput;
}
END_REGION;
if (enabled) {
float input = pidInput->PIDGet();
float result;
PIDOutput* pidOutput;
{
Synchronized sync(m_semaphore);
m_error = m_setpoint - input;
if (m_continuous) {
if (std::fabs(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 (m_I != 0) {
double potentialIGain = (m_totalError + m_error * dt) * m_I;
if (potentialIGain < m_maximumOutput) {
if (potentialIGain > m_minimumOutput)
m_totalError += m_error * dt;
else
m_totalError = m_minimumOutput / m_I;
} else {
m_totalError = m_maximumOutput / m_I;
}
}
// Update deltaError for use in OnTarget()
m_deltaError = (m_error - m_prevError) / dt;
m_result = m_P * m_error + m_I * m_totalError + m_D * m_deltaError +
m_F * m_setpoint;
m_prevError = m_error;
if (m_result > m_maximumOutput)
m_result = m_maximumOutput;
else if (m_result < m_minimumOutput)
m_result = m_minimumOutput;
pidOutput = m_pidOutput;
result = m_result;
}
pidOutput->PIDWrite(result);
}
m_loopTimer.Reset();
}
/**
* Read the input, calculate the output accordingly, and write to the output.
* This should only be called by the Notifier indirectly through CallCalculate
* and is created during initialization.
*/
void PIDController::Calculate()
{
bool enabled;
PIDSource *pidInput;
CRITICAL_REGION(m_semaphore)
{
if (m_pidInput == 0) return;
if (m_pidOutput == 0) return;
enabled = m_enabled;
pidInput = m_pidInput;
}
END_REGION;
if (enabled)
{
float input = pidInput->PIDGet();
float setpoint, result, error;
PIDOutput *pidOutput;
{
Synchronized sync(m_semaphore);
m_error = m_setpoint - input;
if (m_continuous)
{
if (fabs(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(m_I != 0)
{
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput)
{
if (potentialIGain > m_minimumOutput)
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) + m_setpoint * m_F;
m_prevError = m_error;
if (m_result > m_maximumOutput) m_result = m_maximumOutput;
else if (m_result < m_minimumOutput) m_result = m_minimumOutput;
pidOutput = m_pidOutput;
result = m_result;
setpoint = m_setpoint;
error = m_error;
}
pidOutput->PIDWrite(result);
CalculateCallback(input, setpoint, result, error);
}
}
