feat: 更新第九章

chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.cpp

@@ -0,0 +1,64 @@
+#include "Arduino.h"
+#include "PidController.h"
+
+// 构造函数，传入三个PID参数
+PidController::PidController(float kp, float ki, float kd)
+{
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+}
+
+float PidController::update(float current)
+{
+    error_ = target_ - current; //  计算error
+
+    error_sum_ += error_; //  计算error_sum,同时限制积分上下限
+    if (error_sum_ > intergral_up_)
+        error_sum_ = intergral_up_;
+    if (error_sum_ < -1 * intergral_up_)
+        error_sum_ = -1 * intergral_up_;
+
+    derror_ = prev_error_ - error_; // 计算误差变化率
+    prev_error_ = error_;           // 方便下次计算使用
+
+    float output = kp_ * error_ + ki_ * error_sum_ + kd_ * derror_;
+
+    if (output > out_max_)
+        output = out_max_;
+    if (output < out_min_)
+        output = out_min_;
+
+    return output;
+}
+
+void PidController::update_target(float target)
+{
+    target_ = target;
+}
+
+void PidController::update_pid(float kp, float ki, float kd)
+{
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+}
+
+void PidController::reset()
+{
+    error_sum_ = 0;
+    prev_error_ = 0;
+    error_ = 0;
+    derror_ = 0;
+    kp_ = 0;
+    ki_ = 0;
+    kd_ = 0;
+    intergral_up_ = 2500;
+    out_min_ = 0;
+    out_max_ = 0;
+}
+void PidController::out_limit(float min, float max)
+{
+    out_min_ = min;
+    out_max_ = max;
+}

chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.h

@@ -0,0 +1,33 @@
+#ifndef __PID_CONTROLLER_H__
+#define __PID_CONTROLLER_H__
+
+class PidController
+{
+public:
+    PidController() = default;
+    PidController(float kp, float ki, float kd);
+
+private:
+    // PID 参数，可以调节的
+    float target_;
+    float out_min_;
+    float out_max_;
+    float kp_;
+    float ki_;
+    float kd_;
+    float intergral_up_ = 2500; // 积分上限
+    // pid 中间过程值
+    float error_;
+    float error_sum_;
+    float derror_;
+    float prev_error_;
+
+public:
+    float update(float current);                   // 提供当前值，返回下次输出值，也就是PID的结果
+    void update_target(float target);              // 更新目标值
+    void update_pid(float kp, float ki, float kd); // 更新PID参数
+    void reset();                                  // 重置PID
+    void out_limit(float min, float max);          // 设置输出限制
+};
+
+#endif // __PID_CONTROLLER_H__