public PIDController(double Kp, double Ki, double Kd, double Kf,
IPIDSource source, IPIDOutput output,
double period)
{
if (source == null)
throw new ArgumentNullException(nameof(source), "Null PIDSource was given");
if (output == null)
throw new ArgumentNullException(nameof(output), "Null PIDOutput was given");
m_controlLoop = new Notifier(CallCalculate, this);
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_ipidInput = source;
m_ipidOutput = output;
m_period = period;
m_controlLoop.StartPeriodic(m_period);
s_instances++;
HLUsageReporting.ReportPIDController(s_instances);
m_toleranceType = ToleranceType.NoTolerance;
}
public PIDController(double Kp, double Ki, double Kd,
IPIDSource source, IPIDOutput output,
double period)
: this(Kp, Ki, Kd, 0.0, source, output, period)
{
}
/// <summary>
/// Creates a new PID object with the given contants for P, I, D and F.
/// </summary>
/// <param name="kp">The proportional coefficient.</param>
/// <param name="ki">The integral coefficient</param>
/// <param name="kd">The derivative coefficient</param>
/// <param name="kf">The feed forward term.</param>
/// <param name="source">The PIDSource object that is used to get values.</param>
/// <param name="output">The PIDOutput object that is set to the output percentage.</param>
/// <param name="period">The loop time for doing calculations.</param>
public PIDController(double kp, double ki, double kd, double kf,
IPIDSource source, IPIDOutput output,
double period)
{
if (source == null)
{
throw new ArgumentNullException(nameof(source), "Null PIDSource was given");
}
if (output == null)
{
throw new ArgumentNullException(nameof(output), "Null PIDOutput was given");
}
CalculateCallback = Calculate;
m_controlLoop = new Notifier(CalculateCallback);
m_setpointTimer = new Timer();
m_setpointTimer.Start();
m_P = kp;
m_I = ki;
m_D = kd;
m_F = kf;
PIDInput = source;
PIDOutput = output;
m_period = period;
m_controlLoop.StartPeriodic(m_period);
s_instances++;
HAL.Base.HAL.Report(ResourceType.kResourceType_PIDController, (byte)s_instances);
m_toleranceType = ToleranceType.NoTolerance;
m_buf = new Queue <double>(m_bufLength + 1);
}
/// <inheritdoc/>
public void Dispose()
{
m_controlLoop.Stop();
lock (this)
{
PIDOutput = null;
PIDInput = null;
m_controlLoop.Dispose();
m_controlLoop = null;
}
Table?.RemoveTableListener(this);
}
/// <summary>
/// Creates a new PID object with the given contants for P, I, D and F.
/// </summary>
/// <param name="kp">The proportional coefficient.</param>
/// <param name="ki">The integral coefficient</param>
/// <param name="kd">The derivative coefficient</param>
/// <param name="kf">The feed forward term.</param>
/// <param name="source">The PIDSource object that is used to get values.</param>
/// <param name="output">The PIDOutput object that is set to the output percentage.</param>
/// <param name="period">The loop time for doing calculations.</param>
public PIDController(double kp, double ki, double kd, double kf,
IPIDSource source, IPIDOutput output,
double period)
{
if (source == null)
throw new ArgumentNullException(nameof(source), "Null PIDSource was given");
if (output == null)
throw new ArgumentNullException(nameof(output), "Null PIDOutput was given");
CalculateCallback = Calculate;
m_controlLoop = new Notifier(CalculateCallback);
m_setpointTimer = new Timer();
m_setpointTimer.Start();
m_P = kp;
m_I = ki;
m_D = kd;
m_F = kf;
PIDInput = source;
PIDOutput = output;
m_period = period;
m_controlLoop.StartPeriodic(m_period);
s_instances++;
HLUsageReporting.ReportPIDController(s_instances);
m_toleranceType = ToleranceType.NoTolerance;
m_buf = new Queue<double>();
}
/// <inheritdoc/>
public void Dispose()
{
m_controlLoop.Stop();
lock (this)
{
PIDOutput = null;
PIDInput = null;
m_controlLoop.Dispose();
m_controlLoop = null;
}
Table?.RemoveTableListener(this);
}
/// <summary>
/// Creates a new PID object with the given contants for P, I, D and F using a 50ms period
/// </summary>
/// <param name="kp">The proportional coefficient.</param>
/// <param name="ki">The integral coefficient</param>
/// <param name="kd">The derivative coefficient</param>
/// <param name="kf">The feed forward term.</param>
/// <param name="source">The PIDSource object that is used to get values.</param>
/// <param name="output">The PIDOutput object that is set to the output percentage.</param>
public PIDController(double kp, double ki, double kd, double kf,
IPIDSource source, IPIDOutput output)
: this(kp, ki, kd, kf, source, output, DefaultPeriod)
{
}
/// <summary>
/// Creates a new PID object with the given contants for P, I and D.
/// </summary>
/// <param name="kp">The proportional coefficient.</param>
/// <param name="ki">The integral coefficient</param>
/// <param name="kd">The derivative coefficient</param>
/// <param name="source">The PIDSource object that is used to get values.</param>
/// <param name="output">The PIDOutput object that is set to the output percentage.</param>
/// <param name="period">The loop time for doing calculations.</param>
public PIDController(double kp, double ki, double kd,
IPIDSource source, IPIDOutput output,
double period)
: this(kp, ki, kd, 0.0, source, output, period)
{
}
public PIDController(double Kp, double Ki, double Kd, double Kf,
IPIDSource source, IPIDOutput output)
: this(Kp, Ki, Kd, Kf, source, output, DefaultPeriod)
{
}
/// <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);
}
/// <summary>
/// Creates a new PID object with the given contants for P, I, D and F using a 50ms period
/// </summary>
/// <param name="kp">The proportional coefficient.</param>
/// <param name="ki">The integral coefficient</param>
/// <param name="kd">The derivative coefficient</param>
/// <param name="kf">The feed forward term.</param>
/// <param name="source">The PIDSource object that is used to get values.</param>
/// <param name="output">The PIDOutput object that is set to the output percentage.</param>
public PIDController(double kp, double ki, double kd, double kf,
IPIDSource source, IPIDOutput output)
: this(kp, ki, kd, kf, source, output, DefaultPeriod)
{
}
/// <summary>
/// Creates a new PID object with the given contants for P, I and D.
/// </summary>
/// <param name="kp">The proportional coefficient.</param>
/// <param name="ki">The integral coefficient</param>
/// <param name="kd">The derivative coefficient</param>
/// <param name="source">The PIDSource object that is used to get values.</param>
/// <param name="output">The PIDOutput object that is set to the output percentage.</param>
/// <param name="period">The loop time for doing calculations.</param>
public PIDController(double kp, double ki, double kd,
IPIDSource source, IPIDOutput output,
double period)
: this(kp, ki, kd, 0.0, source, output, period)
{
}
