private void NPCTurn()
{
if (isPhysicsSimple)
{
rb.MoveRotation(rb.rotation * Quaternion.Euler(0f, currentTurn * Time.deltaTime, 0f));
}
else
{
float dt = Time.fixedDeltaTime;
float steer = wheelColliderFL.steerAngle;
// using (System.IO.StreamWriter w = System.IO.File.AppendText("/home/hadi/pid.txt"))
// {
// w.WriteLine(steer - targetTurn);
// }
float deltaAngle = -steer_pid.Run(dt, steer, targetTurn);
if (Mathf.Abs(deltaAngle) > maxSteerRate * dt)
{
deltaAngle = Mathf.Sign(deltaAngle) * maxSteerRate * dt;
}
steer += deltaAngle;
steer = Mathf.Min(steer, maxSteeringAngle);
steer = Mathf.Max(steer, -maxSteeringAngle);
wheelColliderFL.steerAngle = steer;
wheelColliderFR.steerAngle = steer;
}
}
private void ApplyTorque()
{
// Maintain speed at target speed
float FRICTION_COEFFICIENT = 0.7f; // for dry wheel/pavement -- wet is about 0.4
speed_pid.UpdateErrors(Time.fixedDeltaTime, currentSpeed_measured, targetSpeed);
float deltaVel = -speed_pid.Run();
float deltaAccel = deltaVel / Time.fixedDeltaTime;
float deltaTorque = 0.25f * rb.mass * Mathf.Abs(Physics.gravity.y) * wheelColliderFR.radius * FRICTION_COEFFICIENT * deltaAccel;
if (deltaTorque > 0)
{
motorTorque += deltaTorque;
if (motorTorque > maxMotorTorque)
{
motorTorque = maxMotorTorque;
}
brakeTorque = 0;
}
else
{
motorTorque = 0;
brakeTorque -= deltaTorque;
if (brakeTorque > maxBrakeTorque)
{
brakeTorque = maxBrakeTorque;
}
}
wheelColliderFR.brakeTorque = brakeTorque;
wheelColliderFL.brakeTorque = brakeTorque;
wheelColliderRL.brakeTorque = brakeTorque;
wheelColliderRR.brakeTorque = brakeTorque;
wheelColliderFR.motorTorque = motorTorque;
wheelColliderFL.motorTorque = motorTorque;
wheelColliderRL.motorTorque = motorTorque;
wheelColliderRR.motorTorque = motorTorque;
}