private void FixedUpdate()
{
for (int i = 0; i < 4; i++)
{
float speed = m_Car.WheelSpeed[i];
if (Mathf.Abs(speed) < m_SpeedThreshold)
{
speed = 0f;
Integral[i] = 0f;
}
float targetAccel;
if (i == 0 || i == 2)
{
targetAccel = Accel - Steering;
}
else
{
targetAccel = Accel + Steering;
}
float targetSpeed = targetAccel * m_TopSpeed;
float newError = targetSpeed - speed;
Proportion[i] = (m_P * newError) / m_TopSpeed;
Integral[i] += (m_I * (newError + Errors[i]) / 2f * Time.deltaTime) / m_TopSpeed;
Derivative[i] = (m_D * (newError - Errors[i]) / Time.deltaTime) / m_TopSpeed;
Proportion[i] = Mathf.Clamp(Proportion[i], -1f, 1f);
// prevent integral wind-up
float result = Proportion[i] + Integral[i];
if (result > 1f)
{
Integral[i] -= result - 1f;
}
else if (result < -1f)
{
Integral[i] -= result + 1f;
}
// apply result
Result[i] = Mathf.Clamp(Proportion[i] + Integral[i] + Derivative[i], -1f, 1f);
m_Car.ApplyTorque(i, Result[i]);
}
}