27 lines
620 B
C
27 lines
620 B
C
/*
|
|
限制器
|
|
*/
|
|
|
|
#include "limiter.h"
|
|
|
|
#include <math.h>
|
|
#include <stddef.h>
|
|
|
|
#include "component/user_math.h"
|
|
|
|
int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, float max_speed, float max_angle, float min_angle){
|
|
|
|
// 限制位置变化速度
|
|
if (*out_pos - feedback_pos > max_speed) {
|
|
*out_pos = feedback_pos + max_speed;
|
|
} else if (*out_pos - feedback_pos < -max_speed) {
|
|
*out_pos = feedback_pos - max_speed;
|
|
}
|
|
// 限制角度范围
|
|
if (*out_pos > max_angle) {
|
|
*out_pos = max_angle;
|
|
} else if (*out_pos < min_angle) {
|
|
*out_pos = min_angle;
|
|
}
|
|
return 0; // 成功
|
|
} |