53 lines
1.2 KiB
C
53 lines
1.2 KiB
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 *out_troque, float max_speed, float max_angle, float min_angle, float max_torque) {
|
|
|
|
// 限制位置变化速度
|
|
// 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 (fabsf(*out_pos - feedback_pos) > max_speed) {
|
|
if (*out_pos > feedback_pos) {
|
|
*out_pos = feedback_pos + max_speed;
|
|
} else {
|
|
*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;
|
|
}
|
|
|
|
/*限制前馈力矩*/
|
|
// if (fabsf(*out_pos) > max_torque) {
|
|
// if (*out_pos > 0) {
|
|
// *out_pos = max_torque;
|
|
// } else {
|
|
// *out_pos = -max_torque;
|
|
// }
|
|
// }
|
|
if (fabsf(*out_troque) > max_torque) {
|
|
if (*out_troque > 0) {
|
|
*out_troque = max_torque;
|
|
} else {
|
|
*out_troque = -max_torque;
|
|
}
|
|
}
|
|
|
|
|
|
return 0; // 成功
|
|
} |