first commit

controllers (2).py

@@ -0,0 +1,145 @@
+# controllers.py
+import os, sys, time
+
+
+def _pid_log(msg: str):
+    """PID 内部日志，直接写文件 + 刷盘"""
+    try:
+        log_dir = os.path.join(os.path.dirname(sys.executable), "logs")
+        os.makedirs(log_dir, exist_ok=True)
+        log_file = os.path.join(log_dir, "control_debug.log")
+        with open(log_file, "a", encoding="utf-8") as f:
+            f.write(f"[{time.strftime('%H:%M:%S.%f')[:-3]}] [PID] {msg}\n")
+            f.flush()
+            os.fsync(f.fileno())
+    except Exception:
+        pass
+
+
+class IncrementalPID:
+    """增量式PID控制器"""
+
+    def __init__(self, kp: float, ki: float, kd: float, dt: float,
+                 out_min: float, out_max: float, xa_full: float = 1062.5):
+        # PID参数
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.dt = dt  # 默认50HZ执行周期防止除零
+        self.motor_max = 300.0
+        self.du_max = self.motor_max * self.dt
+        self.dead_area = 240.0
+        self.xa_full = xa_full  # 总限幅（最大行程）
+
+        # self.kp = 1.392
+        # self.ki = 30.2
+        # self.kd = 0.000485
+        # self.dt = 0.0059
+
+        # 限幅设置
+        self.out_min = out_min
+        self.out_max = out_max
+
+        # 输入输出
+        self.target_pressure = 0.0  # 参考值（设定压力大小）
+        self.current_pressure = 0.0  # 反馈值
+        self.error = 0.0  # 当前误差
+
+        # 计算系数
+        self.a0 = 0.0
+        self.a1 = 0.0
+        self.a2 = 0.0
+        self._calculate_coefficients()
+
+        # 控制器状态
+        self.prev_error = 0.0  # 前次误差 e(k-1)
+        self.prev_error2 = 0.0  # 前前次误差 e(k-2)
+        self.output = 0.0  # 控制器总输出
+
+    def _calculate_coefficients(self):
+        """重新计算增量式PID系数"""
+        if self.dt <= 0:
+            return
+        self.a0 = self.kp + (self.ki * self.dt / 2.0) + (2.0 * self.kd / self.dt)
+        self.a1 = -self.kp + (self.ki * self.dt / 2.0) - (4.0 * self.kd / self.dt)
+        self.a2 = (2.0 * self.kd) / self.dt
+
+    def update_pressure_values(self, current_pressure, target_pressure):
+        """更新当前压力和目标压力值"""
+        self.current_pressure = current_pressure
+        self.target_pressure = target_pressure
+
+    def update(self, du_max=None):
+        # 计算当前误差
+        self.error = -(self.target_pressure - self.current_pressure)
+
+        # if abs(self.error) < 1:
+        #     return self.output  # 误差过小，直接返回当前输出
+
+        # 计算控制增量
+        delta = (self.a0 * self.error + self.a1 * self.prev_error + self.a2 * self.prev_error2)
+
+        if du_max is not None:
+            self.du_max = du_max
+        else:
+            self.du_max = self.get_du_max(self.target_pressure)
+
+        # 纯 Python 限幅（替代 np.clip）
+        if delta > self.du_max:
+            delta = self.du_max
+        elif delta < -self.du_max:
+            delta = -self.du_max
+
+        # 计算新输出
+        new_output = self.output + delta
+        # print(f"output:{self.output}, delta:{delta}, new_output:{new_output}")
+
+        # 应用输出限幅
+        new_output = max(self.out_min, min(self.out_max, new_output))
+
+        # 更新历史状态
+        self.prev_error2 = self.prev_error
+        self.prev_error = self.error
+        self.output = new_output
+        return new_output
+
+    def reset(self):
+        """重置PID控制器状态（保留参数）"""
+        self.prev_error = 0.0
+        self.prev_error2 = 0.0
+        self.output = 0.0
+
+    def update_parameters(self, kp: float, ki: float, kd: float):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self._calculate_coefficients()
+
+    def set_du_max(self, value):
+        """设置 du_max（供外部模块通过方法调用设置，避免跨 .pyd 属性写入 crash）"""
+        self.du_max = value
+
+    def get_du_max(self, target_pressure):
+        """根据目标压力计算 PID 最大增量限幅（纯 Python 线性插值）"""
+        x = float(target_pressure)
+        dt = float(self.dt)
+
+        if x <= 0.0:
+            val = 500.0 * dt
+
+        elif x >= 200.0:
+            val = 250.0 * dt
+
+        elif x <= 100.0:
+            # 0~100：从 500 线性下降到 300
+            val = (500.0 - 2.0 * x) * dt
+
+        else:
+            # 100~200：从 300 线性下降到 250
+            val = (350.0 - 0.5 * x) * dt
+
+        return val
+
+    def init_v(self, position_x):
+        v = (self.xa_full - position_x) / (self.xa_full - self.dead_area) * 100
+        return v