Replace PID with proportional steering, asymmetric servo limits 1.20~1.80ms, center 1.50ms

control/servo.cpp

@@ -4,73 +4,30 @@
 #include <cstdio>
 #include <cmath>
 
-// ── 全局对象 ──────────────────────────────────────────
+static PWM* servo_pwm = nullptr;
-static PWM* servo_pwm = nullptr;      // pwmchip1/pwm0, 3ms 周期
-static PID* servo_pid = nullptr;      // 位置式 PID, 偏差→脉宽偏移量 (ns)
 static bool serv_initialized = false;
 
-// ── 运行时热调参数 ──────────────────────────────────
+static float g_deadband   = 0.03f;    // 死区 (归一化偏差)
-// 这些值会被 scheduler.cpp 每 1 秒从文件热读覆盖
+static float g_steer_gain = 1.5f;     // 转向增益
-static float g_deadband   = 0.03f;    // 死区 (归一化偏差, 默认 ±0.03)
+static constexpr float HALF_RANGE = 300000.0f;  // 半行程 (ns), ±0.30ms
-static float g_steer_gain = 1.5f;     // 转向增益 (偏差放大系数)
 
-// ============================================================
-// 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<float>(SERVO_MAX_DELTA_NS),
-                        static_cast<float>(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
 
-// ============================================================
+// 纯比例转向: deviation(-1~+1) → 线性映射到舵机脉宽范围
-// servo_set_angle — 偏差 → 舵机脉宽 (核心控制回路)
+// deviation=0   → 中位 1.50ms
-//
+// deviation=±1  → 极限 1.20ms / 1.80ms
-// 调用频率: scheduler.cpp sched_control() 每 5ms 一次
+// steer_gain 控制灵敏度 (1.0=标准, 1.5=灵敏, 0.5=柔和)
-//
-// 输入: 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
-// ============================================================
 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 offset = deviation * g_steer_gain * HALF_RANGE;
-    float out = -servo_pid->update(scaled);  // 负号: 偏差正(右偏)→输出负→左转回正
+    float duty_ns = SERVO_CENTER_NS + offset;
-    float duty_ns = SERVO_CENTER_NS + out;
+    duty_ns = std::clamp(duty_ns, 1200000.0f, 1800000.0f);
-    duty_ns = std::clamp(duty_ns,
-                         static_cast<float>(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS),
-                         static_cast<float>(SERVO_CENTER_NS + SERVO_MAX_DELTA_NS));
     servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
 }
 
-// servo_center — 紧急回中 (停止时调用)
 void servo_center()
 {
     if (serv_initialized)

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);
+void servo_set_pid(double kp, double ki, double kd);  // 保留兼容, 空实现
-
-inline float servo_get_duty_ns() { return SERVO_CENTER_NS; }

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

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_CENTER_NS       = 1500000;  // 舵机中位 1.50ms
-constexpr int SERVO_MAX_DELTA_NS    = 210000;   // 舵机偏离中位的最大行程 ±0.21ms
+constexpr int SERVO_MAX_DELTA_NS    = 280000;  // 舵机 PID 输出限幅 (不直接对应物理限位)
 
 // =========================================================
 // IMU — JY62 六轴陀螺仪