Initial commit: SmartCar Framework v0.1 — 龙芯2K0300智能车开发框架\n\n- HAL: GPIO/PWM/Encoder/Framebuffer 驱动\n- Control: PID/IMU/Motor/Servo 控制\n- Vision: HSV双Otsu→4点标定IPM→洪泛填充→逐行搜线\n- Strategy: 三区前瞻偏差+速度策略\n- Debug: 文件热调参+LCD预览+cv截帧\n- Scheduler: 5ms timerfd+epoll 中央调度器

control/servo.cpp

@@ -0,0 +1,57 @@
+#include "servo.hpp"
+#include "hal/pwm.hpp"
+#include <algorithm>
+#include <cstdio>
+#include <cmath>
+
+static PWM* servo_pwm = nullptr;
+static PID* servo_pid = nullptr;
+static bool serv_initialized = false;
+
+static float g_deadband   = 0.03f;
+static float g_steer_gain = 1.5f;
+
+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_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 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<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));
+}
+
+void servo_center()
+{
+    if (serv_initialized)
+        servo_pwm->setDutyCycle(SERVO_CENTER_NS);
+}