添加了pid以及gui

src/PidController.cpp

@@ -0,0 +1,87 @@
+#include "PidController.h"
+#include <algorithm>
+#include <cmath>
+
+PidController::PidController(float kp, float ki, float kd) 
+    : kp_(kp), ki_(ki), kd_(kd), last_error_(0.0f), integral_(0.0f), output_(0.0f), 
+      output_min_(-100.0f), output_max_(100.0f), first_iteration_(true) {}
+
+float PidController::update(float setpoint, float measured_value, float dt) {
+    float error = setpoint - measured_value;
+    
+    // 处理积分饱和
+    integral_ += error * dt;
+    
+    // 计算微分（使用前向差分）
+    float derivative = 0.0f;
+    if (!first_iteration_) {
+        derivative = (error - last_error_) / dt;
+    } else {
+        first_iteration_ = false;
+    }
+    
+    // PID计算
+    float proportional = kp_ * error;
+    float integral_contribution = ki_ * integral_;
+    float derivative_contribution = kd_ * derivative;
+    
+    output_ = proportional + integral_contribution + derivative_contribution;
+    
+    // 限制输出
+    output_ = std::max(output_min_, std::min(output_max_, output_));
+    
+    // 保存当前误差用于下次计算微分
+    last_error_ = error;
+    
+    return output_;
+}
+
+void PidController::reset() {
+    last_error_ = 0.0f;
+    integral_ = 0.0f;
+    output_ = 0.0f;
+    first_iteration_ = true;
+}
+
+void PidController::setKp(float kp) {
+    kp_ = kp;
+}
+
+void PidController::setKi(float ki) {
+    ki_ = ki;
+}
+
+void PidController::setKd(float kd) {
+    kd_ = kd;
+}
+
+void PidController::setParameters(float kp, float ki, float kd) {
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+}
+
+float PidController::getKp() const {
+    return kp_;
+}
+
+float PidController::getKi() const {
+    return ki_;
+}
+
+float PidController::getKd() const {
+    return kd_;
+}
+
+void PidController::setOutputLimits(float min, float max) {
+    output_min_ = min;
+    output_max_ = max;
+}
+
+float PidController::getOutput() const {
+    return output_;
+}
+
+float PidController::getError() const {
+    return last_error_;
+}