R2_UP/User/Algorithm/user_math.h

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

#ifndef USER_MATH_H
#define USER_MATH_H



#include "stm32f4xx.h"
#define ARM_MATH_CM4

#include <float.h>
#include <math.h>
#include <stdbool.h>
#include "struct_typedef.h"

#define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f)

#ifndef M_PI
#define M_PI 3.1415926f
#endif

#ifndef M_2PI
#define M_2PI 6.2831853f
#endif

#define max(a, b)           \
  ({                        \
    __typeof__(a) _a = (a); \
    __typeof__(b) _b = (b); \
    _a > _b ? _a : _b;      \
  })

#define min(a, b)           \
  ({                        \
    __typeof__(a) _a = (a); \
    __typeof__(b) _b = (b); \
    _a < _b ? _a : _b;      \
  })

/* 移动向量 */
typedef struct {
  float vx; /* 前后平移 */
  float vy; /* 左右平移 */
  float wz; /* 转动 */
} MoveVector_t;

//存储极坐标的结构体
typedef struct  {
    fp32 r;  // 极径 
    fp32 theta;  // 极角
}PolarCoordinate_t;

// 表示直角坐标矢量的结构体
typedef  struct  {
    fp32 x;  // x
    fp32 y;   // y
}CartesianVector_t;
//定义获取角度的量纲
typedef enum {
	  DEGREE,//0-360
	  RADIAN,//(0-2pi)
}Angle_e;

//定义获取角度时的角度种类 相对和绝对角度
typedef enum {
	  RELATIVE,
	  ABSOLUTE,
}AngleType_e;


float InvSqrt(float x);
void abs_limit_fp(fp32 *num, fp32 Limit);
fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue);

float AbsClip(float in, float limit);

void Clip(float *origin, float min, float max);

float Sign(float in);

fp32 AngleCalc(AngleType_e type, fp32 abs_angle,fp32 zero_point);

fp32 AngleChange(Angle_e hopetype,fp32 angle);

float _normalizeAngle(float angle);

float _normalizeTo360(float angle);

float calculate_rotation(float current_angle, float target_angle);
	
int map_int(int x, int in_min, int in_max, int out_min, int out_max);

fp32 map_fp32(fp32 x, fp32 in_min, fp32 in_max, fp32 out_min, fp32 out_max);


float abs_float_single (float a);

float abs_float_double(float a,float b);
/**
 * \brief 将运动向量置零
 *
 * \param mv 被操作的值
 */
void ResetMoveVector(MoveVector_t *mv);

/**
 * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
 * 例如编码器：相差1.5PI其实等于相差-0.5PI
 *
 * \param sp 被操作的值
 * \param fb 变化量
 * \param range 被操作的值变化范围，正数时起效
 *
 * \return 函数运行结果
 */
float CircleError(float sp, float fb, float range);

/**
 * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
 * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
 *
 * \param origin 被操作的值
 * \param delta 变化量
 * \param range 被操作的值变化范围，正数时起效
 */
void CircleAdd(float *origin, float delta, float range);

/**
 * @brief 循环值取反
 *
 * @param origin 被操作的值
 */
void CircleReverse(float *origin);

/**
 * @brief 根据目标弹丸速度计算摩擦轮转速
 *
 * @param bullet_speed 弹丸速度
 * @param fric_radius 摩擦轮半径
 * @param is17mm 是否为17mm
 * @return 摩擦轮转速
 */
float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);


// 函数用于将直角坐标转换为极坐标
PolarCoordinate_t cartesianToPolar(fp32 x, fp32 y);

//相加两个极坐标向量并返回结果
PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2);

/// @brief 
/// @param arr 数组
/// @param n 元素数
/// @return 平均值
uint8_t average(uint8_t arr[], uint8_t n);
#endif