Skip to main content

通用工具函数

本例程演示如何使用 librm 中的通用工具函数进行数值处理、角度计算和类型转换。

这些工具函数提供了常用的数学运算和数据处理功能。

符号函数 Sign​

返回数值的符号：正数返回 1，负数返回 -1，零返回 0。

#include <librm.hpp>

int main() {
  int sign1 = rm::modules::Sign(5.0f);    // 1
  int sign2 = rm::modules::Sign(-3.5f);   // -1
  int sign3 = rm::modules::Sign(0.0f);    // 0

  // 可用于改变数值方向
  float value = 10.0f;
  int direction = GetDirection();  // 返回 1 或 -1
  float result = value * direction;

  return 0;
}

死区函数 Deadline​

如果输入值在指定范围内，返回输入值；否则返回 0。

#include <librm.hpp>

void ProcessJoystick() {
  float joystick_x = GetJoystickX();  // -1.0 到 1.0

  // 设置死区：-0.1 到 0.1 之间的输入视为 0
  float filtered_x = rm::modules::Deadline(joystick_x, -0.1f, 0.1f);

  // 现在微小的摇杆抖动会被过滤掉
  if (filtered_x != 0.0f) {
    MoveRobot(filtered_x);
  }
}

限幅函数 Clamp​

将输入值限制在指定范围内，超出范围返回最近的边界值。

#include <librm.hpp>

void ControlMotor() {
  float pid_output = CalculatePID();

  // 限制输出在 -16384 到 16384 之间
  float limited = rm::modules::Clamp(pid_output, -16384.0f, 16384.0f);

  motor.SetCurrent(static_cast<int16_t>(limited));
}

// 限制角度范围
void LimitGimbalAngle() {
  float target_angle = GetTargetAngle();

  // 限制云台角度在 -30° 到 30° 之间
  float safe_angle = rm::modules::Clamp(target_angle, -30.0f, 30.0f);

  gimbal.SetAngle(safe_angle);
}

循环限制函数 Wrap​

将输入值循环限制在一个周期内，常用于角度归一化。

#include <librm.hpp>

void NormalizeAngle() {
  float angle = GetRawAngle();  // 可能是 370°, -10° 等

  // 将角度限制在 0° 到 360° 范围内
  float normalized = rm::modules::Wrap(angle, 0.0f, 360.0f);
  // 370° -> 10°
  // -10° -> 350°

  // 或者限制在 -180° 到 180° 范围内
  float centered = rm::modules::Wrap(angle, -180.0f, 180.0f);
  // 370° -> 10°
  // 190° -> -170°
}

// 编码器值循环处理
void ProcessEncoder() {
  int encoder = GetEncoderValue();  // 0-8191

  // 计算连续角度（可能超过一圈）
  static int last_encoder = 0;
  static int total_rounds = 0;

  int diff = encoder - last_encoder;
  if (diff > 4096) {
    total_rounds--;  // 反向过零
  } else if (diff < -4096) {
    total_rounds++;  // 正向过零
  }

  float continuous_angle = total_rounds * 360.0f +
                          (encoder / 8191.0f) * 360.0f;

  last_encoder = encoder;
}

四元数转欧拉角​

将四元数表示的姿态转换为欧拉角（roll, pitch, yaw）。

#include <librm.hpp>

void ProcessIMU() {
  // 从 IMU 获取四元数
  float quaternion[4] = {q0, q1, q2, q3};  // w, x, y, z

  // 转换为欧拉角（弧度）
  float euler[3];  // roll, pitch, yaw
  rm::modules::QuatToEuler(quaternion, euler);

  float roll = euler[0];   // 横滚角
  float pitch = euler[1];  // 俯仰角
  float yaw = euler[2];    // 偏航角

  // 转换为度
  float yaw_deg = yaw * 180.0f / 3.14159f;
}

区间映射 Map​

将一个范围内的值线性映射到另一个范围。

#include <librm.hpp>

void MapValues() {
  // ADC 值（0-4095）映射到电压（0-3.3V）
  uint16_t adc_value = ReadADC();
  float voltage = rm::modules::Map(
    adc_value, 0.0f, 4095.0f, 0.0f, 3.3f
  );

  // 摇杆值（-660 到 660）映射到速度（-3.0 到 3.0 m/s）
  int16_t joystick = GetJoystick();
  float velocity = rm::modules::Map(
    joystick, -660.0f, 660.0f, -3.0f, 3.0f
  );

  // 角度（0° 到 180°）映射到舵机 PWM（500 到 2500）
  float angle = GetTargetAngle();
  uint16_t pwm = static_cast<uint16_t>(
    rm::modules::Map(angle, 0.0f, 180.0f, 500.0f, 2500.0f)
  );
}

整数与浮点数转换​

在有限位数的整数和浮点数范围之间转换。

#include <librm.hpp>

// 大疆电机位置控制（类似 MIT 模式）
void ControlMITMode() {
  // 电机位置范围：-12.5 到 12.5 rad
  // 编码到 16 位整数（0-65535）

  float target_pos = 3.14f;  // 目标位置（弧度）

  // 浮点转整数
  int pos_int = rm::modules::FloatToInt(
    target_pos, -12.5f, 12.5f, 16
  );
  // pos_int 会是 0 到 65535 之间的值

  // 发送到电机...

  // 接收电机反馈
  int feedback_int = ReceiveMotorFeedback();  // 0-65535

  // 整数转浮点
  float actual_pos = rm::modules::IntToFloat(
    feedback_int, -12.5f, 12.5f, 16
  );
  // actual_pos 会是 -12.5 到 12.5 之间的浮点数
}

// 自定义精度编码
void CustomEncoding() {
  // 12 位精度（0-4095）
  float temperature = 25.6f;  // 温度范围：-40°C 到 125°C

  int temp_encoded = rm::modules::FloatToInt(
    temperature, -40.0f, 125.0f, 12
  );

  // 传输或存储 temp_encoded（只需 12 位）

  // 解码
  float temp_decoded = rm::modules::IntToFloat(
    temp_encoded, -40.0f, 125.0f, 12
  );
}

安全除法 SafeDiv​

执行浮点数除法时考虑除零情况。如果发生除零，则把分母替换为一个很小的数，防止程序崩溃。

#include <librm.hpp>

void CalculateRatio() {
  float numerator = GetSensorValue();
  float denominator = GetDivisor();

  // 普通除法可能除零崩溃
  // float ratio = numerator / denominator;  // 危险！

  // 安全除法：分母为 0 时自动用 1e-6 代替
  float ratio = rm::modules::SafeDiv(numerator, denominator);

  // 现在不会产生 inf 或 NaN
}

// 计算平均值
void CalculateAverage(const float *data, size_t count) {
  float sum = 0.0f;
  for (size_t i = 0; i < count; i++) {
    sum += data[i];
  }

  // 即使 count 为 0 也不会崩溃
  float average = rm::modules::SafeDiv(sum, static_cast<float>(count));
}