53 lines
1.6 KiB
C++
53 lines
1.6 KiB
C++
#include "servo.hpp"
|
|
#include "hal/pwm.hpp"
|
|
#include <algorithm>
|
|
#include <cstdio>
|
|
#include <cmath>
|
|
|
|
static PWM* servo_pwm = nullptr;
|
|
static bool serv_initialized = false;
|
|
|
|
static float g_deadband = 0.03f; // 死区 (归一化偏差)
|
|
static float g_steer_gain = 1.5f; // 转向增益
|
|
static constexpr float HALF_RANGE = 300000.0f; // 半行程 (ns), ±0.30ms
|
|
|
|
void servo_init()
|
|
{
|
|
if (serv_initialized) return;
|
|
servo_pwm = new PWM(SERVO_PWMCHIP, SERVO_PWMID, SERVO_PERIOD_NS);
|
|
servo_pwm->setDutyCycle(SERVO_CENTER_NS);
|
|
servo_pwm->enable();
|
|
serv_initialized = true;
|
|
}
|
|
|
|
void servo_set_deadband(float v) { g_deadband = std::max(0.0f, v); }
|
|
void servo_set_steer_gain(float v) { g_steer_gain = std::max(0.1f, v); }
|
|
void servo_set_pid(double, double, double) {} // 保留接口兼容, 不再使用 PID
|
|
|
|
// 纯比例转向: deviation(-1~+1) → 线性映射到舵机脉宽范围
|
|
// deviation=0 → 中位 1.50ms
|
|
// deviation=±1 → 极限 1.20ms / 1.80ms
|
|
// steer_gain 控制灵敏度 (1.0=标准, 1.5=灵敏, 0.5=柔和)
|
|
void servo_set_angle(float deviation)
|
|
{
|
|
if (!serv_initialized) return;
|
|
deviation = std::clamp(deviation, -1.0f, 1.0f);
|
|
|
|
if (std::abs(deviation) < g_deadband)
|
|
{
|
|
servo_pwm->setDutyCycle(SERVO_CENTER_NS);
|
|
return;
|
|
}
|
|
|
|
float offset = deviation * g_steer_gain * HALF_RANGE;
|
|
float duty_ns = SERVO_CENTER_NS + offset;
|
|
duty_ns = std::clamp(duty_ns, 1200000.0f, 1800000.0f);
|
|
servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
|
|
}
|
|
|
|
void servo_center()
|
|
{
|
|
if (serv_initialized)
|
|
servo_pwm->setDutyCycle(SERVO_CENTER_NS);
|
|
}
|