init

Robot_V2/Algorithm/pid.cpp

@@ -0,0 +1,60 @@
+#include "pid.h"
+#define _constrain(amt, low, high)                                             \
+	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
+#include "Can.h"
+#include "math.h"
+
+extern CAN_transmiter CAN0_transmiter;
+constexpr float TIME_SLICE = 1e-4;
+
+PIDController::PIDController(float P, float I, float D, float ramp, float limit)
+	: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
+	  ,
+	  limit(limit) // PID控制器输出限幅
+	  ,
+	  error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {}
+
+// PID 控制器函数
+float PIDController::operator()(float error) {
+	// P环
+	float proportional = P * error;
+	
+	// Tustin 散点积分（I环）
+	// 添加积分限幅，防止积分饱和
+	float integral = integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev);
+	integral = _constrain(integral, -limit, limit);
+	
+	// D环（微分环节）
+	float derivative = D * (error - error_prev) / TIME_SLICE;
+	
+	// 将P,I,D三环的计算值加起来
+	float output = proportional + integral + derivative;
+	output = _constrain(output, -limit, limit);
+
+	// 输出斜坡限制
+    // 简化的斜坡限制（单行逻辑）
+    if (output_ramp > 0) {
+        float max_delta = output_ramp * TIME_SLICE;
+        output = output_prev + fmaxf(-max_delta, fminf(max_delta, output - output_prev));
+    }
+	// 保存值（为了下一次循环）
+	integral_prev = integral;
+	output_prev = output;
+	error_prev = error;
+
+	return output;
+}
+
+/**
+ * @brief 设置PID参数
+ *
+ * @param P: P参数
+ * @param I: I参数
+ * @param D: D参数
+ * @return void
+ */
+void PIDController::set_PID(float P, float I, float D) {
+	this->P = P;
+	this->I = I;
+	this->D = D;
+}