R2_UP/User/Algorithm/user_math.c

/*
  自定义的数学运算。
*/

#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;
}