PIDAm.py

#PID控制器核心逻辑
"""
PIDAm.py
PID 控制器实现类，支持角度误差修正和输出限幅。
对应原Java文件：PIDAm.java
"""

import time
class PIDAm:
    """
    PID 控制器类，提供带积分限幅和角度误差修正的PID计算功能。

    属性：
        _kp (float): 比例系数
        _ki (float): 积分系数
        _kd (float): 微分系数
        _target (float): 目标值
        _current_value (float): 当前实际值
        _is_angle (bool): 是否为角度控制模式
        _err_last (float): 上一次的误差值
        _err (float): 当前误差值
        _integral (float): 积分累积项
        _max_out (float): 输出上限(默认1.0)
        _min_out (float): 输出下限(默认-1.0)
    """

    def __init__(self, p: float, i: float, d: float):
        """
        初始化PID控制器

        参数：
            p (float): 比例系数
            i (float): 积分系数
            d (float): 微分系数
        """
        # 初始化PID系数
        self._kp = p
        self._ki = i
        self._kd = d

        # 初始化控制器状态变量
        self._dt = 0.0  # 时间间隔
        self._target = 0.0  # 目标值
        self._current_value = 0.0  # 当前值
        self._is_angle = False  # 是否为角度控制模式

        # 输出限制参数
        self._max_out = 1.0  # 输出上限
        self._min_out = -1.0  # 输出下限

        # 控制中间变量
        self._err_last = 0.0  # 上一次误差
        self._err = 0.0  # 当前误差
        self._integral = 0.0  # 积分累积项

    def pid_reset(self) -> None:
        """
        重置控制器状态
        将所有中间变量归零，恢复初始状态
        """
        self._dt = 0.0
        self._target = 0.0
        self._err_last = 0.0
        self._err = 0.0
        self._integral = 0.0
        self._current_value = 0.0
    def pid_realize(self, dt: float, target: float, current_value: float, is_angle: bool = False) -> float:
        """
        执行PID计算

        参数：
            dt (float): 时间间隔(秒)
            target (float): 目标设定值
            current_value (float): 当前实际值
            is_angle (bool): 是否为角度控制模式(默认False)

        返回：
            float: PID控制器输出值(已做限幅处理)

        算法流程：
            1. 计算当前误差(角度模式需特殊处理)
            2. 积分项累积与限幅
            3. 微分项计算
            4. 计算输出并限幅
            5. 保存当前误差供下次使用
        """
        # 更新状态参数
        self._dt = dt
        self._target = target
        self._current_value = current_value
        self._is_angle = is_angle

        # ------------------------- 误差计算 -------------------------
        # 普通模式直接计算误差
        if not self._is_angle:
            self._err = self._target - self._current_value
        # 角度模式处理360°循环问题
        else:
            current_error = self._target - self._current_value
            if current_error >= 180.0:
                self._err = current_error - 360.0
            elif current_error <= -180.0:
                self._err = current_error + 360.0
            else:
                self._err = current_error

        # ------------------------- 积分项处理 -----------------------
        # self._integral += self._err * self._dt
        # # 积分限幅(-1到1)
        # self._integral = max(min(self._integral, 1.0), -1.0)

        # ------------------------- 微分项处理 -----------------------
        # 计算微分项(避免除零错误)
        derivative = 0.0
        if self._dt > 1e-6:  # 时间间隔大于1微秒时计算
            derivative = (self._err - self._err_last) / self._dt

        # ------------------------- PID输出计算 ---------------------
        output = (self._kp * self._err) + \
                 (self._ki * self._integral) + \
                 (self._kd * derivative)

        # 输出限幅
        output_clamped = max(min(output, self._max_out), self._min_out)

        # ------------------------- 抗积分饱和处理 ---------------------
        if output == output_clamped:
            # 未饱和时正常累积积分
            self._integral += self._err * dt
        else:
            # 饱和时反向调整积分（仅当ki非零时）
            if self._ki != 0:
                # 计算饱和误差并补偿积分项
                saturation_error = (output_clamped - output) / self._ki
                self._integral += saturation_error
            else:
                pass

        # 积分项限幅（范围扩大以提高抗饱和能力）
        self._integral = max(min(self._integral, 1.0), -1.0)
        # 日志
        # with open("pid_logafter.csv", "a") as f:
        #     f.write(
        #         f"{time.time()},{self._err},{self._integral},{output},{output_clamped}\n"
        #     )
        return output_clamped

    #没有PID抗积分饱和机制
    # def pid_realize(self, dt: float, target: float, current_value: float, is_angle: bool = False) -> float:
    #     """
    #     执行PID计算
    #
    #     参数：
    #         dt (float): 时间间隔(秒)
    #         target (float): 目标设定值
    #         current_value (float): 当前实际值
    #         is_angle (bool): 是否为角度控制模式(默认False)
    #
    #     返回：
    #         float: PID控制器输出值(已做限幅处理)
    #
    #     算法流程：
    #         1. 计算当前误差(角度模式需特殊处理)
    #         2. 积分项累积与限幅
    #         3. 微分项计算
    #         4. 计算输出并限幅
    #         5. 保存当前误差供下次使用
    #     """
    #     # 更新状态参数
    #     self._dt = dt
    #     self._target = target
    #     self._current_value = current_value
    #     self._is_angle = is_angle
    #
    #     # ------------------------- 误差计算 -------------------------
    #     # 普通模式直接计算误差
    #     if not self._is_angle:
    #         self._err = self._target - self._current_value
    #     # 角度模式处理360°循环问题
    #     else:
    #         current_error = self._target - self._current_value
    #         if current_error >= 180.0:
    #             self._err = current_error - 360.0
    #         elif current_error <= -180.0:
    #             self._err = current_error + 360.0
    #         else:
    #             self._err = current_error
    #
    #     # ------------------------- 积分项处理 -----------------------
    #     self._integral += self._err * self._dt
    #     # 积分限幅(-1到1)
    #     self._integral = max(min(self._integral, 1.0), -1.0)
    #
    #     # ------------------------- 微分项处理 -----------------------
    #     # 计算微分项(避免除零错误)
    #     derivative = 0.0
    #     if self._dt > 1e-6:  # 时间间隔大于1微秒时计算
    #         derivative = (self._err - self._err_last) / self._dt
    #
    #     # ------------------------- PID输出计算 ---------------------
    #     output = (self._kp * self._err) + \
    #              (self._ki * self._integral) + \
    #              (self._kd * derivative)
    #
    #     # 输出限幅
    #     output = max(min(output, self._max_out), self._min_out)
    #
    #     # 保存当前误差供下次使用
    #     self._err_last = self._err
    #     # 日志
    #     with open("pid_logafter.csv", "a") as f:
    #         f.write(
    #             f"{time.time()},{self._err},{self._integral},{output},{output_clamped}\n"
    #         )
    #
    #     return output

    def __str__(self) -> str:
        """返回控制器当前状态字符串"""
        return f"PID[kp={self._kp:.2f}, ki={self._ki:.2f}, kd={self._kd:.2f}] " \
               f"Err: {self._err:.2f}, Integral: {self._integral:.2f}"