麦克纳姆轮动力学模型

class MotorModel:
    def __init__(self, max_rpm=300, gear_ratio=30, torque_constant=0.1):
        """
        简化的电机模型
        max_rpm: 电机最大转速
        gear_ratio: 减速比
        torque_constant: 扭矩常数 Nm/A
        """
        self.max_rpm = max_rpm
        self.gear_ratio = gear_ratio
        self.kt = torque_constant

    def rpm_to_pwm(self, target_rpm, current_rpm=0):
        """
        转速到PWM的转换（简化线性模型）
        实际中可能需要考虑电机特性曲线
        """
        # 计算负载率
        load_percent = target_rpm / self.max_rpm

        # 基础PWM + 补偿项
        base_pwm = 100  # 50%占空比
        pwm_adjustment = load_percent * 100

        # 考虑惯性补偿
        inertia_comp = 0.1 * (target_rpm - current_rpm)

        pwm = base_pwm + pwm_adjustment + inertia_comp

        # 限制范围
        return max(-255, min(255, pwm))

    def calculate_torque(self, current):
        """计算扭矩"""
        return self.kt * current * self.gear_ratio

class MotorController:
    def __init__(self, motor_id, kp=1.0, ki=0.1, kd=0.05):
        self.motor_id = motor_id
        self.kp = kp
        self.ki = ki
        self.kd = kd

        # 状态变量
        self.prev_error = 0
        self.integral = 0
        self.prev_rpm = 0

        # 电机模型
        self.motor = MotorModel()

    def update(self, target_rpm, current_rpm, dt):
        """PID控制更新"""
        error = target_rpm - current_rpm

        # P项
        p_term = self.kp * error

        # I项（抗饱和）
        self.integral += error * dt
        self.integral = max(-1000, min(1000, self.integral))  # 积分限幅
        i_term = self.ki * self.integral

        # D项
        d_term = self.kd * (error - self.prev_error) / dt
        self.prev_error = error

        # 计算输出
        pid_output = p_term + i_term + d_term

        # 转换为PWM
        pwm = self.motor.rpm_to_pwm(target_rpm + pid_output, current_rpm)

        return pwm