161 lines
3.5 KiB
C
161 lines
3.5 KiB
C
/*
|
||
自定义的数学运算。
|
||
*/
|
||
|
||
#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
|