PID.drive C# (CSharp) Method

    public float drive(float p_error, float p_dt)
    {
        float oldError = m_P;
        m_P = p_error; // store current error
        m_I += m_P * p_dt;  // accumulate error velocity to integral term
        m_D = (m_P - oldError) / Mathf.Max(0.001f, p_dt); // calculate speed of error change
        if (float.IsNaN(m_D))
            Debug.Log(m_P + " " + oldError + " " + p_dt);
        // return weighted sum
        return m_Kp * m_P + m_Ki * m_I + m_Kd * m_D;
    }

    // Update is called once per frame
    void Update()
    {
        float current   = transform.rotation.eulerAngles.z;
        float goalAngle = goal.rotation.eulerAngles.z;

        torque = m_pid.drive(Mathf.DeltaAngle(current, goalAngle), Time.deltaTime);
    }