void DoAutoTuneAxis(PIDController pid, int index, Vector3d torque, Vector3d moi) { var accelerationInv = moi [index] / torque [index]; // Don't tune when the available acceleration is less than 0.001 radian.s^-2 if (accelerationInv > 1000) { return; } var kp = twiceZetaOmega [index] * accelerationInv; var ki = omegaSquared [index] * accelerationInv; pid.SetParameters(kp, ki, 0, -1, 1); }