312 lines
6.6 KiB
C
312 lines
6.6 KiB
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;
|
||
}
|