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