using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace ControlEngineering
{
/// <summary>
/// 差分方程式を計算する汎用ベースクラス
/// a[0]y[n] + a[1]y[n-1] + ... = b[0]u[n] + b[1]u[n-1] + ...
/// という一般化された離散時間モデルをシミュレーションします。
/// </summary>
public abstract class DifferenceEquation
{
// 出力 y の係数配列 (a[0], a[1], a[2] ...)
protected double[] a;
// 入力 u の係数配列 (b[0], b[1], b[2] ...)
protected double[] b;
// サンプリング周期 (Δt)
private double _dt;
public double Dt
{
get => _dt;
set
{
if (value <= 0) throw new ArgumentException("Dt must be greater than 0.");
_dt = value;
UpdateCoefficients(); // Δtが変われば係数も変わるため再計算
}
}
protected DifferenceEquation(double dt)
{
_dt = dt;
}
/// <summary>
/// パラメータが変更された際に、a配列とb配列の係数を再計算する抽象メソッド
/// </summary>
protected abstract void UpdateCoefficients();
/// <summary>
/// 入力配列 u を受け取り、差分方程式に従って出力配列 y を計算して返します。
/// </summary>
public double[] Simulate(double[] u)
{
if (a == null || b == null || a.Length == 0)
throw new InvalidOperationException("Coefficients are not initialized.");
int len = u.Length;
double[] y = new double[len];
int orderY = a.Length - 1;
int orderU = b.Length - 1;
// 時系列ループ (n=0 から開始)
for (int n = 0; n < len; n++)
{
double sumU = 0;
// b[0]*u[n] + b[1]*u[n-1] + ... を計算
for (int i = 0; i <= orderU; i++)
{
if (n - i >= 0)
sumU += b[i] * u[n - i];
}
double sumY = 0;
// a[1]*y[n-1] + a[2]*y[n-2] + ... を計算 (a[0]は含まない)
for (int i = 1; i <= orderY; i++)
{
if (n - i >= 0)
sumY += a[i] * y[n - i];
}
// y[n] について解く: y[n] = (sumU - sumY) / a[0]
y[n] = (sumU - sumY) / a[0];
}
return y;
}
}
/// <summary>
/// 一次遅れ系 (First-Order Lag System)
/// G(s) = K / (Ts + 1)
/// </summary>
public class FirstOrderLag : DifferenceEquation
{
private double _k;
private double _t;
public double K
{
get => _k;
set { _k = value; UpdateCoefficients(); }
}
public double T
{
get => _t;
set { _t = value; UpdateCoefficients(); }
}
public FirstOrderLag(double k, double t, double dt) : base(dt)
{
_k = k;
_t = t;
UpdateCoefficients();
}
protected override void UpdateCoefficients()
{
// T * (y[n] - y[n-1])/Dt + y[n] = K * u[n]
// (T + Dt)*y[n] - T*y[n-1] = (K * Dt)*u[n]
a = new double[2];
a[0] = T + Dt;
a[1] = -T;
b = new double[1];
b[0] = K * Dt;
}
}
/// <summary>
/// 二次遅れ系 (Second-Order Lag System)
/// 標準形パラメータ(K, ωn, ζ)と物理パラメータ(m, c, k)が連動します。
/// </summary>
public class SecondOrderLag : DifferenceEquation
{
// --- 標準形パラメータ ---
private double _kGain;
private double _omegaN;
private double _zeta;
// --- 物理パラメータ ---
private double _m;
private double _c;
private double _kSpring; // クラス名のK(ゲイン)と区別するため kSpring と命名
// プロパティの相互更新による無限ループを防ぐフラグ
private bool _isSyncing = false;
#region Properties (Standard Form)
public double K
{
get => _kGain;
set { _kGain = value; SyncPhysicalFromStandard(); UpdateCoefficients(); }
}
public double OmegaN
{
get => _omegaN;
set { _omegaN = value; SyncPhysicalFromStandard(); UpdateCoefficients(); }
}
public double Zeta
{
get => _zeta;
set { _zeta = value; SyncPhysicalFromStandard(); UpdateCoefficients(); }
}
#endregion
#region Properties (Physical Parameters)
public double M
{
get => _m;
set { _m = value; SyncStandardFromPhysical(); UpdateCoefficients(); }
}
public double C
{
get => _c;
set { _c = value; SyncStandardFromPhysical(); UpdateCoefficients(); }
}
public double KSpring
{
get => _kSpring;
set { _kSpring = value; SyncStandardFromPhysical(); UpdateCoefficients(); }
}
#endregion
/// <summary>
/// 標準形パラメータ (K, ωn, ζ) を指定して初期化
/// </summary>
public SecondOrderLag(double kGain, double omegaN, double zeta, double dt) : base(dt)
{
_kGain = kGain;
_omegaN = omegaN;
_zeta = zeta;
SyncPhysicalFromStandard();
UpdateCoefficients();
}
/// <summary>
/// K, ωn, ζ の値から m, c, kSpring を計算して同期する
/// </summary>
private void SyncPhysicalFromStandard()
{
if (_isSyncing) return;
_isSyncing = true; // ロック開始
_kSpring = 1.0 / _kGain;
_m = _kSpring / (_omegaN * _omegaN);
_c = 2.0 * _zeta * Math.Sqrt(_m * _kSpring);
_isSyncing = false; // ロック解除
}
/// <summary>
/// m, c, kSpring の値から K, ωn, ζ を計算して同期する
/// </summary>
private void SyncStandardFromPhysical()
{
if (_isSyncing) return;
_isSyncing = true; // ロック開始
_kGain = 1.0 / _kSpring;
_omegaN = Math.Sqrt(_kSpring / _m);
_zeta = _c / (2.0 * Math.Sqrt(_m * _kSpring));
_isSyncing = false; // ロック解除
}
protected override void UpdateCoefficients()
{
// (1 + 2*ζ*ωn*Δt + ωn^2*Δt^2) * y[n] - (2 + 2*ζ*ωn*Δt) * y[n-1] + 1 * y[n-2] = (K * ωn^2 * Δt^2) * u[n]
double dt2 = Dt * Dt;
double wn2 = OmegaN * OmegaN;
double term = 2.0 * Zeta * OmegaN * Dt;
a = new double[3];
a[0] = 1.0 + term + wn2 * dt2;
a[1] = -(2.0 + term);
a[2] = 1.0;
b = new double[1];
b[0] = K * wn2 * dt2;
}
}
/// <summary>
/// ローパスフィルタ (Low-Pass Filter)
/// 数学的には一次遅れ系と同じですが、カットオフ周波数(Fc)で指定できるようにしたクラスです。
/// G(s) = 1 / (Ts + 1) ただし T = 1 / (2πFc)
/// </summary>
public class LowPassFilter : DifferenceEquation
{
private double _fc;
/// <summary>
/// カットオフ周波数 [Hz]
/// </summary>
public double Fc
{
get => _fc;
set { _fc = value; UpdateCoefficients(); }
}
public LowPassFilter(double fc, double dt) : base(dt)
{
_fc = fc;
UpdateCoefficients();
}
protected override void UpdateCoefficients()
{
// 時定数 T をカットオフ周波数から計算
double T = 1.0 / (2.0 * Math.PI * Fc);
a = new double[2];
a[0] = T + Dt;
a[1] = -T;
b = new double[1];
b[0] = Dt; // フィルタなので定常ゲイン K=1.0
}
}
/// <summary>
/// PIDコントローラ
/// 後退差分を用いた速度型アルゴリズムの式を、差分方程式の一般形に展開したものです。
/// </summary>
public class PIDController : DifferenceEquation
{
private double _kp;
private double _ki;
private double _kd;
public double Kp
{
get => _kp;
set { _kp = value; UpdateCoefficients(); }
}
public double Ki
{
get => _ki;
set { _ki = value; UpdateCoefficients(); }
}
public double Kd
{
get => _kd;
set { _kd = value; UpdateCoefficients(); }
}
public PIDController(double kp, double ki, double kd, double dt) : base(dt)
{
_kp = kp;
_ki = ki;
_kd = kd;
UpdateCoefficients();
}
protected override void UpdateCoefficients()
{
// PIDの連続式: U(s) = (Kp + Ki/s + Kd*s) * E(s)
// 後退差分による速度型PIDアルゴリズムを y[n], y[n-1], u[n], u[n-1], u[n-2] の係数に変換
// (ここではベースクラスの汎用性を保つため、入力uを偏差(Error)、出力yを操作量(MV)としています)
a = new double[2];
a[0] = 1.0;
a[1] = -1.0;
b = new double[3];
b[0] = Kp + Ki * Dt + Kd / Dt;
b[1] = -(Kp + 2.0 * Kd / Dt);
b[2] = Kd / Dt;
}
}
}