电机参数

电流环带宽

atan2

// based on https://math.stackexchange.com/a/1105038/81278
float fast_atan2(float y, float x) {// a := min (|x|, |y|) / max (|x|, |y|)float abs_y = fabsf(y);float abs_x = fabsf(x);// inject FLT_MIN in denominator to avoid division by zerofloat a = MACRO_MIN(abs_x, abs_y) / (MACRO_MAX(abs_x, abs_y) + FLT_MIN);// s := a * afloat s = a * a;// r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + afloat r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;// if |y| > |x| then r := 1.57079637 - rif (abs_y > abs_x)r = 1.57079637f - r;// if x < 0 then r := 3.14159274 - rif (x < 0.0f)r = 3.14159274f - r;// if y < 0 then r := -rif (y < 0.0f)r = -r;return r;
}

// Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
int mod(int dividend, int divisor){int r = dividend % divisor;return (r < 0) ? (r + divisor) : r;
}