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;
}
/// <summary>
/// Creates a linear FIR or IIR filter.
/// </summary>
/// <param name="source">The <see cref="IPIDSource"/> object that is used to get values.</param>
/// <param name="ffGains">The "feed forward" or FIR gains.</param>
/// <param name="fbGains">The "feed back" or IIR gains.</param>
public LinearDigitalFilter(IPIDSource source, double[] ffGains, double[] fbGains) : base(source)
{
m_inputs = new CircularBuffer<double>(ffGains.Length);
m_outputs = new CircularBuffer<double>(fbGains.Length);
m_inputGains = ffGains;
m_outputGains = fbGains;
}
/// <summary>
/// Creates a linear FIR or IIR filter.
/// </summary>
/// <param name="source">The <see cref="IPIDSource"/> object that is used to get values.</param>
/// <param name="ffGains">The "feed forward" or FIR gains.</param>
/// <param name="fbGains">The "feed back" or IIR gains.</param>
public LinearDigitalFilter(IPIDSource source, double[] ffGains, double[] fbGains) : base(source)
{
m_inputs = new CircularBuffer <double>(ffGains.Length);
m_outputs = new CircularBuffer <double>(fbGains.Length);
m_inputGains = ffGains;
m_outputGains = fbGains;
}
/// <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);
}
public PIDController(double Kp, double Ki, double Kd,
IPIDSource source, IPIDOutput output,
double period)
: this(Kp, Ki, Kd, 0.0, source, output, period)
{
}
/// <summary>
/// Ceates a one-pole IIR low-passs filter.
/// </summary>
/// <remarks>
/// The filter is in the form
/// y[n] = gain*x[n] + (1-gain)*y[n-1].
/// <para></para>
/// This filter is stable for gains in the range [0, 1]
/// </remarks>
/// <param name="source">The <see cref="IPIDSource"/> object that is used to get values.</param>
/// <param name="timeConstant">The filter's feedforward gain factor (lower = smoother but slower).</param>
/// <param name="period"></param>
/// <returns>A new Single Pole IIR <see cref="LinearDigitalFilter"/></returns>
// ReSharper disable once InconsistentNaming
public static LinearDigitalFilter SinglePoleIIR(IPIDSource source, double timeConstant,
double period)
{
double gain = Math.Exp(-period / timeConstant);
double[] ffGains = { 1.0 - gain };
double[] fbGains = { -gain };
return(new LinearDigitalFilter(source, ffGains, fbGains));
}
/// <summary>
/// Creates a first-order high-pass filter.
/// </summary>
/// <remarks>
/// The filter is in the form
/// y[n] = gain*x[n] + (-gain)*x[n-1] + gain*y[n-1]
/// where gain = e^(-dt / T), T is the time constant in seconds.
/// <para>This filter is stable for time constants greater then zero.</para>
/// </remarks>
/// <param name="source">The <see cref="IPIDSource"/> object that is used to get values.</param>
/// <param name="timeConstant">The discrete-time time constant in seconds.</param>
/// <param name="period">The period in seconds between samples taken by the user.</param>
/// <returns></returns>
public static LinearDigitalFilter HighPass(IPIDSource source, double timeConstant,
double period)
{
double gain = Math.Exp(-period / timeConstant);
double[] ffGains = { gain, -gain };
double[] fbGains = { -gain };
return(new LinearDigitalFilter(source, ffGains, fbGains));
}
/// <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 K-tap FIR moving average filter.
/// </summary>
/// <remarks>
/// The filter is in the form
/// y[n] = 1/k * (x[k] + x[k-1] + ... + x[0]).
/// <para></para>
/// This filter is always stable
/// </remarks>
/// <param name="source">The <see cref="IPIDSource"/> object that is used to get values.</param>
/// <param name="taps">The number of samples to average over. Higher = smoother but slower</param>
/// <returns>A new Moving Average <see cref="LinearDigitalFilter"/></returns>
public static LinearDigitalFilter MovingAverage(IPIDSource source, int taps)
{
if (taps <= 0)
{
throw new ArgumentOutOfRangeException(nameof(taps), "Number of taps was not at least 1.");
}
double[] ffGains = new double[taps];
for (int i = 0; i < ffGains.Length; i++)
{
ffGains[i] = 1.0 / taps;
}
double[] fbGains = new double[0];
return(new LinearDigitalFilter(source, ffGains, fbGains));
}
/// <summary>
/// Creates a new <see cref="Filter"/>
/// </summary>
/// <param name="source">The <see cref="IPIDSource"/> to filter.</param>
protected Filter(IPIDSource source)
{
m_source = source;
}
/// <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)
{
}
/// <summary>
/// Ceates a one-pole IIR low-passs filter.
/// </summary>
/// <remarks>
/// The filter is in the form
/// y[n] = gain*x[n] + (1-gain)*y[n-1].
/// <para></para>
/// This filter is stable for gains in the range [0, 1]
/// </remarks>
/// <param name="source">The <see cref="IPIDSource"/> object that is used to get values.</param>
/// <param name="timeConstant">The filter's feedforward gain factor (lower = smoother but slower).</param>
/// <param name="period"></param>
/// <returns>A new Single Pole IIR <see cref="LinearDigitalFilter"/></returns>
public static LinearDigitalFilter SinglePoleIIR(IPIDSource source, double timeConstant,
double period)
{
double gain = Math.Exp(-period / timeConstant);
double[] ffGains = { 1.0 - gain };
double[] fbGains = { -gain };
return new LinearDigitalFilter(source, ffGains, fbGains);
}
/// <summary>
/// Creates a new <see cref="Filter"/>
/// </summary>
/// <param name="source">The <see cref="IPIDSource"/> to filter.</param>
protected Filter(IPIDSource source)
{
m_source = source;
}
/// <summary>
/// Creates a K-tap FIR moving average filter.
/// </summary>
/// <remarks>
/// The filter is in the form
/// y[n] = 1/k * (x[k] + x[k-1] + ... + x[0]).
/// <para></para>
/// This filter is always stable
/// </remarks>
/// <param name="source">The <see cref="IPIDSource"/> object that is used to get values.</param>
/// <param name="taps">The number of samples to average over. Higher = smoother but slower</param>
/// <returns>A new Moving Average <see cref="LinearDigitalFilter"/></returns>
public static LinearDigitalFilter MovingAverage(IPIDSource source, int taps)
{
if (taps <= 0)
{
throw new ArgumentOutOfRangeException(nameof(taps), "Number of taps was not at least 1.");
}
double[] ffGains = new double[taps];
for (int i = 0; i < ffGains.Length; i++)
{
ffGains[i] = 1.0 / taps;
}
double[] fbGains = new double[0];
return new LinearDigitalFilter(source, ffGains, fbGains);
}
/// <summary>
/// Creates a first-order high-pass filter.
/// </summary>
/// <remarks>
/// The filter is in the form
/// y[n] = gain*x[n] + (-gain)*x[n-1] + gain*y[n-1]
/// where gain = e^(-dt / T), T is the time constant in seconds.
/// <para>This filter is stable for time constants greater then zero.</para>
/// </remarks>
/// <param name="source">The <see cref="IPIDSource"/> object that is used to get values.</param>
/// <param name="timeConstant">The discrete-time time constant in seconds.</param>
/// <param name="period">The period in seconds between samples taken by the user.</param>
/// <returns></returns>
public static LinearDigitalFilter HighPass(IPIDSource source, double timeConstant,
double period)
{
double gain = Math.Exp(-period / timeConstant);
double[] ffGains = { gain, -gain };
double[] fbGains = { -gain };
return new LinearDigitalFilter(source, ffGains, fbGains);
}
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)
{
}
/// <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)
{
}
