void Update()
{
m_entries = new List <Entry>();
int n = (int)(totalTime / timeStep) + 1;
for (int i = 0; i < n; i++)
{
float target = startTarget * (i < n / 2 ? 0.5f : 1f);
var entry = new Entry()
{
t = i * timeStep,
target = target
};
m_entries.Add(entry);
}
m_entries[0].x = startValue;
m_pid = new ScalarPID();
m_pid.P = m_p;
m_pid.I = m_i;
m_pid.D = m_d;
m_pid.DampingI = m_dampingI;
m_pid.ClampingI = m_clampingI;
for (int i = 0; i < n - 1; i++)
{
m_entries[i].x += m_entries[i].v * timeStep;
float prevTarget = i == 0 ? 0f : m_entries[i - 1].target;
float target = m_entries[i].target;
float error = m_entries[i].target - m_entries[i].x;
float dx = m_pid.Step(error, prevTarget - target, timeStep);
m_entries[i].dx = dx;
m_entries[i].pv = m_pid.ValueP;
m_entries[i].iv = m_pid.ValueI;
m_entries[i].dv = m_pid.ValueD;
m_entries[i + 1].v = m_entries[i].v + Mathf.Clamp(dx, 0, maxMotorForce) * timeStep;
m_entries[i + 1].x += m_entries[i].x;
m_entries[i + 1].v += externalForce * timeStep;
}
}
