R2_UP/User/Algorithm/user_math.c

312 lines
6.6 KiB
C
Raw Normal View History

2025-03-12 10:46:02 +08:00
/*
*/
#include "user_math.h"
#include "math.h"
#include <string.h>
/**
* \brief
*
* \param x
* \param in_min
* \param in_min
* \param in_min
* \param in_min
*
* \return
*/
int map_int(int x, int in_min, int in_max, int out_min, int out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
fp32 map_fp32(fp32 x, fp32 in_min, fp32 in_max, fp32 out_min, fp32 out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
int Setint_Deadzone(int value,int deadzone)
{
if(value>=-deadzone&&value<=deadzone)
{
value =0;
}
return 1;
}
float abs_float_double(float a,float b){
if((a-b)<0){return (b-a);}
else if((a>b)>0){return (a-b);}
else return 0;
}
float abs_float_single (float a){
if(a<0) return -a;
else if(a>0) return a;
else return 0;
}
// 归一化角度到 [0,2PI]
float _normalizeAngle(float angle)
{
float a = fmod(angle, 2*M_PI); //取余运算可以用于归一化,列出特殊值例子算便知
return a >= 0 ? a : (a + 2*M_PI);
}
//归一化角度到 [0,360]
float _normalizeTo360(float angle)
{
// 使用 fmod 函数取 angle 除以 360 的余数
float a = fmod(angle, 360.0f);
// 如果余数小于 0则加上 360 再返回
return a < 0 ? (a + 360.0f) : a;
}
//角度量纲的转换
fp32 AngleChange(Angle_e hopetype,fp32 angle)
{
fp32 hope_angle;
if(hopetype==DEGREE)
{
hope_angle = angle*180/M_PI; //弧度制转角度制
}
else if(hopetype ==RADIAN)
{
hope_angle = angle*M_PI/180; //角度值转弧度制
}
return hope_angle;
}
//获取角度的函数
/*
param in type: RELATIVE ABSOLUTE
abs_angle:
zero_point:
param out final_angle:
*/
fp32 AngleCalc(AngleType_e type, fp32 abs_angle,fp32 zero_point)
{
fp32 final_angle=0;
if(type==RELATIVE)
{
if(abs_angle - zero_point >=0.0f)
{
final_angle = abs_angle - zero_point;
}
else final_angle = M_2PI +(abs_angle -zero_point);
}
else if(type ==ABSOLUTE)
{
final_angle = abs_angle;
}
return final_angle;
}
// 函数:计算最短路径需要转动的角度
float calculate_rotation(float current_angle, float target_angle) {
float diff = target_angle - current_angle;
// 将差值调整到-180到180之间
if (diff > 180.0f) {
diff -= 360.0f;
} else if (diff < -180.0f) {
diff += 360.0f;
}
return diff;
}
//相加两个极坐标向量并返回结果
PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2) {
// 将极坐标转换为直角坐标
double x1, y1, x2, y2;
x1 = v1.r * cos(v1.theta);
y1 = v1.r * sin(v1.theta);
x2 = v2.r * cos(v2.theta);
y2 = v2.r * sin(v2.theta);
// 直角坐标相加
double x_sum = x1 + x2;
double y_sum = y1 + y2;
// 将直角坐标结果转换回极坐标
PolarCoordinate_t sum;
sum.r = sqrt(x_sum * x_sum + y_sum * y_sum);
sum.theta = atan2(y_sum, x_sum);
return sum;
}
//函数用于将直角坐标转换为极坐标
PolarCoordinate_t cartesianToPolar(fp32 x, fp32 y) {
PolarCoordinate_t polar;
// 计算极径
polar.r = sqrt(x * x + y * y);
// 计算极角
if (x == 0) {
if (y > 0) {
polar.theta = M_PI / 2.0f;
} else if (y < 0) {
polar.theta = -M_PI / 2.0f;
} else {
polar.theta = 0; // 任何角度都可以表示为0
}
} else {
polar.theta = atan2(y, x);
}
return polar;
}
//循环限幅
fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue)
{
if (maxValue < minValue)
{
return Input;
}
if (Input > maxValue)
{
fp32 len = maxValue - minValue;
while (Input > maxValue)
{
Input -= len;
}
}
else if (Input < minValue)
{
fp32 len = maxValue - minValue;
while (Input < minValue)
{
Input += len;
}
}
return Input;
}
//限幅
void abs_limit_fp(fp32 *num, fp32 Limit)
{
if (*num > Limit)
{
*num = Limit;
}
else if (*num < -Limit)
{
*num = -Limit;
}
}
/* 移动向量 */
MoveVector_t *mv;
inline float InvSqrt(float x) {
#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
}
inline float AbsClip(float in, float limit) {
return (in < -limit) ? -limit : ((in > limit) ? limit : in);
}
inline void Clip(float *origin, float min, float max) {
if (*origin > max) *origin = max;
if (*origin < min) *origin = min;
}
inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
/**
* \brief
*
* \param mv
*/
inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
/**
* \brief
* 1.5PI其实等于相差-0.5PI
*
* \param sp
* \param fb
* \param range
*
* \return
*/
inline float CircleError(float sp, float fb, float range) {
float error = sp - fb;
if (range > 0.0f) {
float half_range = range / 2.0f;
if (error > half_range)
error -= range;
else if (error < -half_range)
error += range;
}
return error;
}
/**
* \brief
* 0-2PI内变化1.5PI + 1.5PI = 1PI
*
* \param origin
* \param delta
* \param range
*/
inline void CircleAdd(float *origin, float delta, float range) {
float out = *origin + delta;
if (range > 0.0f) {
if (out >= range)
out -= range;
else if (out < 0.0f)
out += range;
}
*origin = out;
}
/**
* @brief
*
* @param origin
*/
inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
/// @brief
/// @param arr 数组
/// @param n 元素数
/// @return 平均值
uint8_t average(uint8_t arr[], uint8_t n) {
int sum = 0;
for (int i = 0; i < n; i++) {
sum += arr[i];
}
return (float) sum / n;
}