140 lines
2.8 KiB
C
140 lines
2.8 KiB
C
#include "calc_lib.h"
|
|
#include <math.h>
|
|
|
|
//΢ÃëÑÓʱ
|
|
void user_delay_us(uint16_t us)
|
|
{
|
|
for(; us > 0; us--)
|
|
{
|
|
for(uint8_t i = 50; i > 0; i--)
|
|
{
|
|
;
|
|
}
|
|
}
|
|
}
|
|
|
|
//ºÁÃëÑÓʱ
|
|
void user_delay_ms(uint16_t ms)
|
|
{
|
|
for(; ms > 0; ms--)
|
|
{
|
|
user_delay_us(1000);
|
|
}
|
|
}
|
|
|
|
//¸¡µãÊý·¶Î§ÏÞÖÆ
|
|
void abs_limit_fp(fp32 *num, fp32 Limit)
|
|
{
|
|
if (*num > Limit)
|
|
{
|
|
*num = Limit;
|
|
}
|
|
else if (*num < -Limit)
|
|
{
|
|
*num = -Limit;
|
|
}
|
|
}
|
|
|
|
//ÕûÊý·¶Î§ÏÞÖÆ
|
|
void abs_limit_int(int64_t *num, int64_t Limit)
|
|
{
|
|
if (*num > Limit)
|
|
{
|
|
*num = Limit;
|
|
}
|
|
else if (*num < -Limit)
|
|
{
|
|
*num = -Limit;
|
|
}
|
|
}
|
|
|
|
//Ñ»·ÏÞ·ù
|
|
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;
|
|
}
|
|
|
|
int32_t loop_int32_constrain(int32_t Input, int32_t minValue, int32_t maxValue)
|
|
{
|
|
if (maxValue < minValue)
|
|
{
|
|
return Input;
|
|
}
|
|
if (Input > maxValue)
|
|
{
|
|
int32_t len = maxValue - minValue;
|
|
while (Input > maxValue)
|
|
{
|
|
Input -= len;
|
|
}
|
|
}
|
|
else if (Input < minValue)
|
|
{
|
|
int32_t len = maxValue - minValue;
|
|
while (Input < minValue)
|
|
{
|
|
Input += len;
|
|
}
|
|
}
|
|
|
|
|
|
return Input;
|
|
}
|
|
|
|
int map(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;
|
|
}
|
|
|
|
// ??????????????????????
|
|
float expo_map(float value, float expo_factor) {
|
|
return value * (1 - expo_factor) + value * fabs(value) * expo_factor;
|
|
}
|
|
|
|
|
|
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) {
|
|
static int count = 0; // 满足条件的计数
|
|
|
|
if (count >= threshold) {
|
|
count=0;//重置
|
|
return true; // 如果已经满足条件，返回 1
|
|
|
|
}
|
|
|
|
// 判断三个值是否都满足条件
|
|
bool all_reached = (fabs(current1 - target1) < mistake) &&
|
|
(fabs(current2 - target2) < mistake) &&
|
|
(fabs(current3 - target3) < mistake);
|
|
|
|
if (all_reached) {
|
|
count++; // 所有条件都满足，计数加 1
|
|
}
|
|
return false; // 未满足条件达到阈值，返回 0
|
|
}
|