/*
  限制器
*/

#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; // 成功
}