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; } }