106 lines
3.5 KiB
C#
106 lines
3.5 KiB
C#
namespace LibDynamics
|
|
{
|
|
public class SecondOrderDynamics
|
|
{
|
|
private double xp;// previous input
|
|
private double y, yd; // state variables
|
|
private double _w, _z, _d, k1, k2, k3; // dynamics constants
|
|
private double _r;
|
|
private double _f;
|
|
|
|
/// <summary>
|
|
/// 频率
|
|
/// - 即速度, 单位是赫兹(Hz)
|
|
/// - 不会影响输出结果的形状, 会影响 '震荡频率'
|
|
/// </summary>
|
|
public double F
|
|
{
|
|
get => _f; set
|
|
{
|
|
_f = value;
|
|
InitMotionValues(_f, _z, _r);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 阻尼 <br />
|
|
/// - 当为 0 时, 输出将永远震荡不衰减 <br />
|
|
/// - 当大于 0 小于 1 时, 输出会超出结果, 并逐渐趋于目标 <br />
|
|
/// - 当为 1 时, 输出的曲线是趋向结果, 并正好在指定频率对应时间内抵达结果 <br />
|
|
/// - 当大于 1 时, 输出值同样时取向结果, 但速度会更慢, 无法在指定频率对应时间内抵达结果 <br />
|
|
/// </summary>
|
|
public double Z
|
|
{
|
|
get => _z; set
|
|
{
|
|
_z = value;
|
|
InitMotionValues(_f, _z, _r);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 初始响应
|
|
/// - 当为 0 时, 数据需要进行 '加速' 来开始运动 <br />
|
|
/// - 当为 1 时, 数据会立即开始响应 <br />
|
|
/// - 当大于 1 时, 输出会因为 '速度过快' 而超出目标结果 <br />
|
|
/// - 当小于 0 时, 输出会 '预测运动', 即 '抬手动作'. 例如目标是 '加' 时, 输出会先进行 '减', 再进行 '加',
|
|
/// - 当运动目标为机械时, 通常取值为 2
|
|
/// </summary>
|
|
public double R
|
|
{
|
|
get => _r; set
|
|
{
|
|
_r = value;
|
|
InitMotionValues(_f, _z, _r);
|
|
}
|
|
}
|
|
|
|
public SecondOrderDynamics(double f, double z, double r, double x0)
|
|
{
|
|
//compute constants
|
|
InitMotionValues(f, z, r);
|
|
|
|
// initialize variables
|
|
xp = x0;
|
|
y = x0;
|
|
yd = 0;
|
|
}
|
|
|
|
private void InitMotionValues(double f, double z, double r)
|
|
{
|
|
_w = 2 * Math.PI * f;
|
|
_z = z;
|
|
_d = _w * Math.Sqrt(Math.Abs(z * z - 1));
|
|
k1 = z / (Math.PI * f);
|
|
k2 = 1 / ((2 * Math.PI * f) * (2 * Math.PI * f));
|
|
k3 = r * z / (2 * Math.PI * f);
|
|
}
|
|
|
|
public double Update(double deltaTime, double x)
|
|
{
|
|
double xd = (x - xp) / deltaTime;
|
|
double k1_stable, k2_stable;
|
|
|
|
if (_w * deltaTime < _z)
|
|
{
|
|
k1_stable = k1;
|
|
k2_stable = Math.Max(Math.Max(k2, deltaTime * deltaTime / 2 + deltaTime * k1 / 2), deltaTime * k1);
|
|
}
|
|
else
|
|
{
|
|
double t1 = Math.Exp(-_z * _w * deltaTime);
|
|
double alpha = 2 * t1 * (_z <= 1 ? Math.Cos(deltaTime * _d) : Math.Cosh(deltaTime * _d));
|
|
double beta = t1 * t1;
|
|
double t2 = deltaTime / (1 + beta - alpha);
|
|
k1_stable = (1 - beta) * t2;
|
|
k2_stable = deltaTime * t2;
|
|
}
|
|
|
|
y = y + deltaTime * yd;
|
|
yd = yd + deltaTime * (x + k3 * xd - y - k1_stable * yd) / k2_stable;
|
|
|
|
xp = x;
|
|
return y;
|
|
}
|
|
}
|
|
} |