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