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

44
component/user_math.c
View File

@ -29,13 +29,13 @@ 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 变化量