6 changed files with 270 additions and 121 deletions
@ -0,0 +1,139 @@ |
|||||
|
using System; |
||||
|
using System.Collections.Generic; |
||||
|
using System.Linq; |
||||
|
using System.Text; |
||||
|
using System.Threading.Tasks; |
||||
|
|
||||
|
namespace DyeingComputer.UserClass |
||||
|
{ |
||||
|
public class PID |
||||
|
{ |
||||
|
private static double Ts; // Sample period in seconds
|
||||
|
private static double K; // Rollup parameter
|
||||
|
private static double b0, b1, b2; // Rollup parameters
|
||||
|
private static double a0, a1, a2; // Rollup parameters
|
||||
|
private static double y0 = 0; // Current output
|
||||
|
private static double y1 = 0; // Output one iteration old
|
||||
|
private static double y2 = 0; // Output two iterations old
|
||||
|
private static double e0 = 0; // Current error
|
||||
|
private static double e1 = 0; // Error one iteration old
|
||||
|
private static double e2 = 0; // Error two iterations old
|
||||
|
|
||||
|
/// <summary>
|
||||
|
/// PID构造器
|
||||
|
/// </summary>
|
||||
|
/// <param name="Kp">比例增益</param>
|
||||
|
/// <param name="Ki">积分增益</param>
|
||||
|
/// <param name="Kd">微分增益</param>
|
||||
|
/// <param name="N">微分滤波系数</param>
|
||||
|
/// <param name="OutputUpperLimit">控制器输出上限</param>
|
||||
|
/// <param name="OutputLowerLimit">控制器输出下限</param>
|
||||
|
public PID(double Kp, double Ki, double Kd, double N, double OutputUpperLimit, double OutputLowerLimit) |
||||
|
{ |
||||
|
this.Kp = Kp; |
||||
|
this.Ki = Ki; |
||||
|
this.Kd = Kd; |
||||
|
this.N = N; |
||||
|
this.OutputUpperLimit = OutputUpperLimit; |
||||
|
this.OutputLowerLimit = OutputLowerLimit; |
||||
|
} |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// PID迭代器，在每个采样周期调用此函数以获取当前控制器输出。
|
||||
|
/// 设定点和过程值应使用相同的单位。
|
||||
|
/// </summary>
|
||||
|
/// <param name="setPoint">设定目标值</param>
|
||||
|
/// <param name="processValue">当前实际值</param>
|
||||
|
/// <param name="ts">自上次迭代以来的时间跨度，对第一次调用使用默认采样周期</param>
|
||||
|
/// <returns>Current Controller Output</returns>
|
||||
|
public double PID_iterate(double setPoint, double processValue, TimeSpan ts) |
||||
|
{ |
||||
|
// Ensure the timespan is not too small or zero.
|
||||
|
Ts = (ts.TotalSeconds >= TsMin) ? ts.TotalSeconds : TsMin; |
||||
|
|
||||
|
// Calculate rollup parameters
|
||||
|
K = 2 / Ts; |
||||
|
b0 = Math.Pow(K, 2) * Kp + K * Ki + Ki * N + K * Kp * N + Math.Pow(K, 2) * Kd * N; |
||||
|
b1 = 2 * Ki * N - 2 * Math.Pow(K, 2) * Kp - 2 * Math.Pow(K, 2) * Kd * N; |
||||
|
b2 = Math.Pow(K, 2) * Kp - K * Ki + Ki * N - K * Kp * N + Math.Pow(K, 2) * Kd * N; |
||||
|
a0 = Math.Pow(K, 2) + N * K; |
||||
|
a1 = -2 * Math.Pow(K, 2); |
||||
|
a2 = Math.Pow(K, 2) - K * N; |
||||
|
|
||||
|
// Age errors and output history
|
||||
|
e2 = e1; // Age errors one iteration
|
||||
|
e1 = e0; // Age errors one iteration
|
||||
|
e0 = setPoint - processValue; // Compute new error
|
||||
|
y2 = y1; // Age outputs one iteration
|
||||
|
y1 = y0; // Age outputs one iteration
|
||||
|
y0 = -a1 / a0 * y1 - a2 / a0 * y2 + b0 / a0 * e0 + b1 / a0 * e1 + b2 / a0 * e2; // Calculate current output
|
||||
|
|
||||
|
// Clamp output if needed
|
||||
|
if (y0 > OutputUpperLimit) |
||||
|
{ |
||||
|
y0 = OutputUpperLimit; |
||||
|
} |
||||
|
else if (y0 < OutputLowerLimit) |
||||
|
{ |
||||
|
y0 = OutputLowerLimit; |
||||
|
} |
||||
|
|
||||
|
return y0; |
||||
|
} |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// 重置控制器历史记录，有效重置控制器。
|
||||
|
/// </summary>
|
||||
|
public static void ResetController() |
||||
|
{ |
||||
|
e2 = 0; |
||||
|
e1 = 0; |
||||
|
e0 = 0; |
||||
|
y2 = 0; |
||||
|
y1 = 0; |
||||
|
y0 = 0; |
||||
|
} |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// 比例增益，如果此参数发生剧烈变化，请考虑重置控制器。
|
||||
|
/// </summary>
|
||||
|
public double Kp { get; set; } |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// 积分增益，如果此参数发生剧烈变化，请考虑重置控制器。
|
||||
|
/// </summary>
|
||||
|
public double Ki { get; set; } |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// 微分增益，如果此参数发生剧烈变化，请考虑重置控制器。
|
||||
|
/// </summary>
|
||||
|
public double Kd { get; set; } |
||||
|
|
||||
|
/// <summary>
|
||||
|
///微分滤波系数。
|
||||
|
/// N越小，过滤效果越好
|
||||
|
/// N越大，过滤效果越差
|
||||
|
/// 如果此参数发生剧烈变化，请考虑重置控制器。
|
||||
|
/// </summary>
|
||||
|
public double N { get; set; } |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// 避免除以零的最小允许采样周期！
|
||||
|
/// Ts值可能会在第一次迭代时被错误地设置为过低的值或零。
|
||||
|
/// TsMin默认设置为1毫秒。
|
||||
|
/// </summary>
|
||||
|
public double TsMin { get; set; } = 0.001; |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// 控制器的输出上限。
|
||||
|
/// 是一个比输出下限大的数值。
|
||||
|
/// </summary>
|
||||
|
public double OutputUpperLimit { get; set; } |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// 控制器的输出下限。
|
||||
|
/// 是一个比输出上限小的数值。
|
||||
|
/// </summary>
|
||||
|
public double OutputLowerLimit { get; set; } |
||||
|
} |
||||
|
} |
||||
Loading…
Reference in new issue