/*
  限制器
*/

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