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/pid.cpp

@@ -0,0 +1,53 @@
+#include "pid.hpp"
+#include <algorithm>
+#include <cmath>
+
+PID::PID(float kp, float ki, float kd, float target,
+         PIDMode mode, float out_limit, float int_limit)
+    : _kp(kp), _ki(ki), _kd(kd), _target(target), _mode(mode)
+    , _out_limit(out_limit), _int_limit(int_limit)
+    , _output(0), _integral(0), _prev_error(0), _last_measured(0), _first(true) {}
+
+void PID::setPID(float kp, float ki, float kd)
+{
+    _kp = kp; _ki = ki; _kd = kd;
+    reset();
+}
+
+void PID::reset()
+{
+    _integral = 0;
+    _prev_error = 0;
+    _output = 0;
+    _first = true;
+}
+
+float PID::update(float measured)
+{
+    float error = _target - measured;
+
+    if (_mode == PID_POSITIONAL)
+    {
+        _integral += error;
+        _integral = std::clamp(_integral, -_int_limit, _int_limit);
+
+        float derivative = 0;
+        if (!_first) derivative = error - _prev_error;
+
+        _output = _kp * error + _ki * _integral + _kd * derivative;
+    }
+    else // PID_INCREMENTAL
+    {
+        float inc = _kp * (error - _prev_error) + _ki * error;
+        if (!_first)
+            inc += _kd * (error - 2.0f * _prev_error + (_prev_error - (_target - _last_measured)));
+        _output += inc;
+    }
+
+    _output = std::clamp(_output, -_out_limit, _out_limit);
+    _prev_error = error;
+    _last_measured = measured;
+    _first = false;
+
+    return _output;
+}