157 lines
3.1 KiB
C
157 lines
3.1 KiB
C
/*
|
||
自定义的数学运算。
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
#include "stm32f4xx.h"
|
||
#define ARM_MATH_CM4
|
||
|
||
#include <float.h>
|
||
#include <math.h>
|
||
#include <stdbool.h>
|
||
|
||
#define M_DEG2RAD_MULT (0.01745329251f)
|
||
#define M_RAD2DEG_MULT (57.2957795131f)
|
||
|
||
#ifndef M_PI
|
||
#define M_PI 3.14159265358979323846f
|
||
#endif
|
||
|
||
#ifndef M_2PI
|
||
#define M_2PI 6.28318530717958647692f
|
||
#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;
|
||
|
||
float InvSqrt(float x);
|
||
|
||
float AbsClip(float in, float limit);
|
||
|
||
void Clip(float *origin, float min, float max);
|
||
|
||
float Sign(float in);
|
||
|
||
/**
|
||
* \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);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
#ifdef DEBUG
|
||
|
||
/**
|
||
* @brief 如果表达式的值为假则运行处理函数
|
||
*
|
||
*/
|
||
#define ASSERT(expr) \
|
||
do { \
|
||
if (!(expr)) { \
|
||
VerifyFailed(__FILE__, __LINE__); \
|
||
} \
|
||
} while (0)
|
||
#else
|
||
|
||
/**
|
||
* @brief 未定DEBUG，表达式不会运行，断言被忽略
|
||
*
|
||
*/
|
||
#define ASSERT(expr) ((void)(0))
|
||
#endif
|
||
|
||
#ifdef DEBUG
|
||
|
||
/**
|
||
* @brief 如果表达式的值为假则运行处理函数
|
||
*
|
||
*/
|
||
#define VERIFY(expr) \
|
||
do { \
|
||
if (!(expr)) { \
|
||
VerifyFailed(__FILE__, __LINE__); \
|
||
} \
|
||
} while (0)
|
||
#else
|
||
|
||
/**
|
||
* @brief 表达式会运行，忽略表达式结果
|
||
*
|
||
*/
|
||
#define VERIFY(expr) ((void)(expr))
|
||
#endif
|
||
|
||
/**
|
||
* @brief 断言失败处理
|
||
*
|
||
* @param file 文件名
|
||
* @param line 行号
|
||
*/
|
||
void VerifyFailed(const char *file, uint32_t line);
|