diff --git a/control/servo.cpp b/control/servo.cpp index 5a89b4b..08ef507 100644 --- a/control/servo.cpp +++ b/control/servo.cpp @@ -4,73 +4,30 @@ #include #include -// ── 全局对象 ────────────────────────────────────────── -static PWM* servo_pwm = nullptr; // pwmchip1/pwm0, 3ms 周期 -static PID* servo_pid = nullptr; // 位置式 PID, 偏差→脉宽偏移量 (ns) +static PWM* servo_pwm = nullptr; static bool serv_initialized = false; -// ── 运行时热调参数 ────────────────────────────────── -// 这些值会被 scheduler.cpp 每 1 秒从文件热读覆盖 -static float g_deadband = 0.03f; // 死区 (归一化偏差, 默认 ±0.03) -static float g_steer_gain = 1.5f; // 转向增益 (偏差放大系数) +static float g_deadband = 0.03f; // 死区 (归一化偏差) +static float g_steer_gain = 1.5f; // 转向增益 +static constexpr float HALF_RANGE = 300000.0f; // 半行程 (ns), ±0.30ms -// ============================================================ -// servo_init — 初始化舵机 PWM + PID -// -// 调用时机: scheduler_init() 启动时调用一次 -// -// 硬件: pwmchip1/pwm0 -// 周期 = 3ms (333Hz) — 舵机标准周期 -// 中位 = 1.52ms — 前轮摆正 -// 行程 = 1.52 ± 0.21ms — 约 ±45° 物理摆角 -// -// PID: 位置式 -// Kp=400000, Ki=200000, Kd=0 -// 输出限幅 = ±SERVO_MAX_DELTA_NS (±210000ns = ±0.21ms) -// 这组参数等效于线性增益 (因为 Ki 很大, 积分瞬间到位) -// ============================================================ 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(); - - servo_pid = new PID(400000, 200000, 0, 0, PID_POSITIONAL, - static_cast(SERVO_MAX_DELTA_NS), - static_cast(SERVO_MAX_DELTA_NS)); - 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 kp, double ki, double kd) { if (servo_pid) servo_pid->setPID(kp, ki, kd); } +void servo_set_pid(double, double, double) {} // 保留接口兼容, 不再使用 PID -// ============================================================ -// servo_set_angle — 偏差 → 舵机脉宽 (核心控制回路) -// -// 调用频率: scheduler.cpp sched_control() 每 5ms 一次 -// -// 输入: deviation — 归一化中线偏差 (-1=偏左极限, 0=正中, +1=偏右极限) -// 由 strategy/deviation.cpp 的 calc_deviation() 计算 -// -// 处理流程: -// 1. 钳位偏差至 [-1, +1] -// 2. 死区: |deviation| < g_deadband → 直接回中, 跳过 PID -// 典型值 g_deadband=0.03, 即偏差 < 3% 时不转向 -// 3. 增益缩放: deviation *= g_steer_gain -// 典型值 g_steer_gain=1.5, 把微小偏差放大供 PID 处理 -// 4. PID 计算: out = PID(deviation) -// 位置式 PID, 输出为脉宽偏移量 (ns) -// 5. 叠加中位: duty_ns = 1520000 + out -// 正偏差(偏右) → out>0 → 脉宽>中位 → 舵机右转 -// 负偏差(偏左) → out<0 → 脉宽<中位 → 舵机左转 -// 6. 限幅: 1.52 ± 0.21ms (1310000ns ~ 1730000ns) -// 7. 写入 PWM sysfs -// ============================================================ +// 纯比例转向: 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; @@ -82,16 +39,12 @@ void servo_set_angle(float deviation) return; } - float scaled = deviation * g_steer_gain; - float out = -servo_pid->update(scaled); // 负号: 偏差正(右偏)→输出负→左转回正 - float duty_ns = SERVO_CENTER_NS + out; - duty_ns = std::clamp(duty_ns, - static_cast(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS), - static_cast(SERVO_CENTER_NS + SERVO_MAX_DELTA_NS)); + 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(duty_ns)); } -// servo_center — 紧急回中 (停止时调用) void servo_center() { if (serv_initialized) diff --git a/control/servo.hpp b/control/servo.hpp index 41b3bc5..5f420c2 100644 --- a/control/servo.hpp +++ b/control/servo.hpp @@ -1,12 +1,9 @@ #pragma once #include "types.hpp" -#include "control/pid.hpp" void servo_init(); void servo_set_angle(float deviation); void servo_center(); void servo_set_deadband(float v); void servo_set_steer_gain(float v); -void servo_set_pid(double kp, double ki, double kd); - -inline float servo_get_duty_ns() { return SERVO_CENTER_NS; } +void servo_set_pid(double kp, double ki, double kd); // 保留兼容, 空实现 diff --git a/ctl.sh b/ctl.sh index cbf7509..700018b 100644 --- a/ctl.sh +++ b/ctl.sh @@ -34,7 +34,7 @@ init_pins() { echo 0 > /sys/class/pwm/pwmchip1/export 2>/dev/null echo 3000000 > /sys/class/pwm/pwmchip1/pwm0/period 2>/dev/null echo 1 > /sys/class/pwm/pwmchip1/pwm0/enable 2>/dev/null - echo 1520000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null + echo 1500000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null echo 12 > "$DIR/speed" 2>/dev/null echo 0.03 > "$DIR/deadband" 2>/dev/null diff --git a/model/test_model b/model/test_model new file mode 100644 index 0000000..faa2b6c Binary files /dev/null and b/model/test_model differ diff --git a/types.hpp b/types.hpp index 1e228d8..71eb13b 100644 --- a/types.hpp +++ b/types.hpp @@ -65,8 +65,8 @@ constexpr int ENCODER_RIGHT_DIR_GPIO= 72; // 右编码器方向检测脚 constexpr int SERVO_PWMCHIP = 1; // 舵机 PWM 芯片 constexpr int SERVO_PWMID = 0; // 舵机 PWM 通道 constexpr int SERVO_PERIOD_NS = 3000000; // PWM 周期 3ms (333Hz) -constexpr int SERVO_CENTER_NS = 1520000; // 舵机中位 1.52ms -constexpr int SERVO_MAX_DELTA_NS = 210000; // 舵机偏离中位的最大行程 ±0.21ms +constexpr int SERVO_CENTER_NS = 1500000; // 舵机中位 1.50ms +constexpr int SERVO_MAX_DELTA_NS = 280000; // 舵机 PID 输出限幅 (不直接对应物理限位) // ========================================================= // IMU — JY62 六轴陀螺仪