robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-11-02 20:43:11 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

更新user_math

Browse Source
This commit is contained in:
Robofish 2025-10-02 21:41:52 +08:00
parent c3faf353e9
commit 17847459b4
2 changed files with 42 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
component/user_math.c
View File

@ -9,33 +9,33 @@
/* USER INCLUDE END */ /* USER INCLUDE END */
inline float InvSqrt(float x) { inline float InvSqrt(float x) {
//#if 0 // #if 0
/* Fast inverse square-root */ /* Fast inverse square-root */
/* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */ /* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */
float halfx = 0.5f * x; float halfx = 0.5f * x;
float y = x; float y = x;
long i = *(long*)&y; long i = *(long *)&y;
i = 0x5f3759df - (i>>1); i = 0x5f3759df - (i >> 1);
y = *(float*)&i; y = *(float *)&i;
y = y * (1.5f - (halfx * y * y)); y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y)); y = y * (1.5f - (halfx * y * y));
return y; return y;
//#else // #else
// return 1.0f / sqrtf(x); // return 1.0f / sqrtf(x);
//#endif // #endif
} }
inline float AbsClip(float in, float limit) { inline float AbsClip(float in, float limit) {
return (in < -limit) ? -limit : ((in > limit) ? limit : in); return (in < -limit) ? -limit : ((in > limit) ? limit : in);
} }
float fAbs(float in){ float fAbs(float in) { return (in > 0) ? in : -in; }
return (in > 0) ? in : -in;
}
inline void Clip(float *origin, float min, float max) { inline void Clip(float *origin, float min, float max) {
if (*origin > max) *origin = max; if (*origin > max)
if (*origin < min) *origin = min; *origin = max;
if (*origin < min)
*origin = min;
} }
inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; } inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
@ -48,13 +48,12 @@ inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); } inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
/** /**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值 * \brief
* 例如编码器：相差1.5PI其实等于相差-0.5PI *计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
* *例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* \param sp 被操作的值 * \param sp 设定值
* \param fb 变化量 * \param fb 反馈值
* \param range 被操作的值变化范围，正数时起效 * \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果 * \return 函数运行结果
*/ */
inline float CircleError(float sp, float fb, float range) { inline float CircleError(float sp, float fb, float range) {
@ -71,9 +70,8 @@ inline float CircleError(float sp, float fb, float range) {
} }
/** /**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值 * \brief 循环加法，适用于被操作的值在（x,y）范围内循环的情况，range应设定为y-x
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
*
* \param origin 被操作的值 * \param origin 被操作的值
* \param delta 变化量 * \param delta 变化量
* \param range 被操作的值变化范围，正数时起效 * \param range 被操作的值变化范围，正数时起效
@ -105,14 +103,20 @@ inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
* @return 摩擦轮转速 * @return 摩擦轮转速
*/ */
inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) { inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
if (bullet_speed == 0.0f) return 0.f; if (bullet_speed == 0.0f)
return 0.f;
if (is17mm) { if (is17mm) {
if (bullet_speed == 15.0f) return 4670.f; if (bullet_speed == 15.0f)
if (bullet_speed == 18.0f) return 5200.f; return 4670.f;
if (bullet_speed == 30.0f) return 7350.f; if (bullet_speed == 18.0f)
return 5200.f;
if (bullet_speed == 30.0f)
return 7350.f;
} else { } else {
if (bullet_speed == 10.0f) return 4450.f; if (bullet_speed == 10.0f)
if (bullet_speed == 16.0f) return 5800.f; return 4450.f;
if (bullet_speed == 16.0f)
return 5800.f;
} }
/* 不为裁判系统设定值时,计算转速 */ /* 不为裁判系统设定值时,计算转速 */

12
component/user_math.h
View File

@ -82,11 +82,11 @@ float Sign(float in);
void ResetMoveVector(MoveVector_t *mv); void ResetMoveVector(MoveVector_t *mv);
/** /**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值 * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
* 例如编码器：相差1.5PI其实等于相差-0.5PI *例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* *
* \param sp 被操作的值 * \param sp 设定值
* \param fb 变化量 * \param fb 反馈值
* \param range 被操作的值变化范围，正数时起效 * \param range 被操作的值变化范围，正数时起效
* *
* \return 函数运行结果 * \return 函数运行结果
@ -94,8 +94,8 @@ void ResetMoveVector(MoveVector_t *mv);
float CircleError(float sp, float fb, float range); float CircleError(float sp, float fb, float range);
/** /**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值 * \brief 循环加法，适用于被操作的值在（x,y）范围内循环的情况，range应设定为y-x
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* *
* \param origin 被操作的值 * \param origin 被操作的值
* \param delta 变化量 * \param delta 变化量