pid_controller.py

"""
PID控制器模块
实现独立的X轴和Y轴PID控制
"""
import time
import math

class PIDController:
    """PID控制器类"""
    
    def __init__(self, kp=0.8, ki=0.2, kd=0.1, max_output=2000, min_output=-2000, 
                 integral_limit=1000, deadband=5):
        """
        初始化PID控制器
        
        参数:
            kp: 比例系数
            ki: 积分系数  
            kd: 微分系数
            max_output: 最大输出限制
            min_output: 最小输出限制
            integral_limit: 积分项限制
            deadband: 死区范围，小于此值输出为0
        """
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.max_output = max_output
        self.min_output = min_output
        self.integral_limit = integral_limit
        self.deadband = deadband
        
        # PID状态变量
        self.last_error = 0.0
        self.integral = 0.0
        self.last_time = time.time()
        
        # 输出限制
        self.last_output = 0.0
        self.max_change_rate = 500  # 最大变化率，防止突变
        
    def reset(self):
        """重置PID控制器状态"""
        self.last_error = 0.0
        self.integral = 0.0
        self.last_time = time.time()
        self.last_output = 0.0
        
    def update(self, setpoint, current_value):
        """
        更新PID控制器
        
        参数:
            setpoint: 目标值
            current_value: 当前值
            
        返回:
            output: 控制输出
        """
        current_time = time.time()
        dt = current_time - self.last_time
        
        # 防止除零
        if dt <= 0:
            dt = 0.01
            
        # 计算误差
        error = setpoint - current_value
        
        # 死区处理
        if abs(error) < self.deadband:
            error = 0.0
            
        # 比例项
        proportional = self.kp * error
        
        # 积分项
        self.integral += error * dt
        # 积分限制
        self.integral = max(-self.integral_limit, 
                           min(self.integral_limit, self.integral))
        integral_term = self.ki * self.integral
        
        # 微分项
        derivative = (error - self.last_error) / dt
        derivative_term = self.kd * derivative
        
        # PID输出
        output = proportional + integral_term + derivative_term
        
        # 输出变化率限制
        max_change = self.max_change_rate * dt
        output_change = output - self.last_output
        if abs(output_change) > max_change:
            output = self.last_output + (max_change if output_change > 0 else -max_change)
        
        # 输出限制
        output = max(self.min_output, min(self.max_output, output))
        
        # 更新状态
        self.last_error = error
        self.last_time = current_time
        self.last_output = output
        
        return output
    
    def set_parameters(self, kp=None, ki=None, kd=None):
        """动态调整PID参数"""
        if kp is not None:
            self.kp = kp
        if ki is not None:
            self.ki = ki
        if kd is not None:
            self.kd = kd


class DualAxisPIDController:
    """双轴PID控制器，分别控制X轴和Y轴"""
    
    def __init__(self, x_pid_params=None, y_pid_params=None):
        """
        初始化双轴PID控制器
        
        参数:
            x_pid_params: X轴PID参数字典
            y_pid_params: Y轴PID参数字典
        """
        # 默认X轴参数
        x_default = {
            'kp': 0.6, 'ki': 0.15, 'kd': 0.08,
            'max_output': 1500, 'min_output': -1500,
            'integral_limit': 800, 'deadband': 8
        }
        
        # 默认Y轴参数  
        y_default = {
            'kp': 0.5, 'ki': 0.12, 'kd': 0.06,
            'max_output': 1500, 'min_output': -1500,
            'integral_limit': 800, 'deadband': 8
        }
        
        # 合并用户参数
        if x_pid_params:
            x_default.update(x_pid_params)
        if y_pid_params:
            y_default.update(y_pid_params)
            
        # 创建X轴和Y轴PID控制器
        self.x_pid = PIDController(**x_default)
        self.y_pid = PIDController(**y_default)
        
        # 目标点
        self.target_x = 0.0
        self.target_y = 0.0
        
    def set_target(self, target_x, target_y):
        """设置目标点"""
        self.target_x = target_x
        self.target_y = target_y
        
    def update(self, current_x, current_y):
        """
        更新双轴PID控制器
        
        参数:
            current_x: 当前X坐标
            current_y: 当前Y坐标
            
        返回:
            (vx, vy): X轴和Y轴的控制输出
        """
        vx = self.x_pid.update(self.target_x, current_x)
        vy = self.y_pid.update(self.target_y, current_y)
        
        return vx, vy
        
    def reset(self):
        """重置两个PID控制器"""
        self.x_pid.reset()
        self.y_pid.reset()
        
    def set_parameters(self, axis, **params):
        """
        设置指定轴的PID参数
        
        参数:
            axis: 'x' 或 'y'
            **params: PID参数字典
        """
        if axis == 'x':
            self.x_pid.set_parameters(**params)
        elif axis == 'y':
            self.y_pid.set_parameters(**params)
        else:
            raise ValueError("axis must be 'x' or 'y'")