robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-11-02 04:23:10 +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 */
inline float InvSqrt(float x) {
//#if 0
// #if 0
/* Fast inverse square-root */
/* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
//#else
// return 1.0f / sqrtf(x);
//#endif
float halfx = 0.5f * x;
float y = x;
long i = *(long *)&y;
i = 0x5f3759df - (i >> 1);
y = *(float *)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
// #else
// return 1.0f / sqrtf(x);
// #endif
}
inline float AbsClip(float in, float limit) {
return (in < -limit) ? -limit : ((in > limit) ? limit : in);
}
float fAbs(float in){
return (in > 0) ? in : -in;
}
float fAbs(float in) { return (in > 0) ? in : -in; }
inline void Clip(float *origin, float min, float max) {
if (*origin > max) *origin = max;
if (*origin < min) *origin = min;
if (*origin > max)
*origin = max;
if (*origin < min)
*origin = min;
}
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)); }
/**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
* 例如编码器：相差1.5PI其实等于相差-0.5PI
*
* \param sp 被操作的值
* \param fb 变化量
* \brief
*计算循环值的误差，适用于设定值与反馈值均在（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 fb 反馈值
* \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果
*/
inline float CircleError(float sp, float fb, float range) {
@ -71,9 +70,8 @@ inline float CircleError(float sp, float fb, float range) {
}
/**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
*
* \brief 循环加法，适用于被操作的值在（x,y）范围内循环的情况，range应设定为y-x
* 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效
@ -105,14 +103,20 @@ inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
* @return 摩擦轮转速
*/
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 (bullet_speed == 15.0f) return 4670.f;
if (bullet_speed == 18.0f) return 5200.f;
if (bullet_speed == 30.0f) return 7350.f;
if (bullet_speed == 15.0f)
return 4670.f;
if (bullet_speed == 18.0f)
return 5200.f;
if (bullet_speed == 30.0f)
return 7350.f;
} else {
if (bullet_speed == 10.0f) return 4450.f;
if (bullet_speed == 16.0f) return 5800.f;
if (bullet_speed == 10.0f)
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);
/**
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
* 例如编码器：相差1.5PI其实等于相差-0.5PI
* \brief 计算循环值的误差，适用于设定值与反馈值均在（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 fb 变化量
* \param sp 设定值
* \param fb 反馈值
* \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果
@ -94,8 +94,8 @@ void ResetMoveVector(MoveVector_t *mv);
float CircleError(float sp, float fb, float range);
/**
* \brief 循环加法，用于没有负数值，并在一定范围内变化的值
* 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
* \brief 循环加法，适用于被操作的值在（x,y）范围内循环的情况，range应设定为y-x
* 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
*
* \param origin 被操作的值
* \param delta 变化量