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>(); }
/// <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 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) { }