麦克纳姆轮运动控制算法

class MecanumBaseController:
    def __init__(self):
        # 运动学参数
        self.kinematics = MecanumKinematics(
            wheel_radius=0.05,
            lx=0.15,  # 假设底盘长30cm
            ly=0.15   # 假设底盘宽30cm
        )

        # 四个电机的控制器
        self.motor_ctrl = [
            MotorController(1, kp=1.5, ki=0.2, kd=0.1),
            MotorController(2, kp=1.5, ki=0.2, kd=0.1),
            MotorController(3, kp=1.5, ki=0.2, kd=0.1),
            MotorController(4, kp=1.5, ki=0.2, kd=0.1)
        ]

        # 状态变量
        self.current_speeds = [0, 0, 0, 0]  # 四个轮子当前转速
        self.target_speeds = [0, 0, 0, 0]   # 目标转速

    def set_chassis_velocity(self, vx, vy, omega):
        """设置底盘速度"""
        # 1. 运动学转换
        w1, w2, w3, w4 = self.kinematics.chassis_to_wheel(vx, vy, omega)

        # 2. 转换为RPM
        self.target_speeds = [
            w1 * 60 / (2 * math.pi),  # rad/s 转 RPM
            w2 * 60 / (2 * math.pi),
            w3 * 60 / (2 * math.pi),
            w4 * 60 / (2 * math.pi)
        ]

    def update(self, dt):
        """控制循环更新"""
        pwm_values = []

        # 对每个电机进行PID控制
        for i in range(4):
            pwm = self.motor_ctrl[i].update(
                self.target_speeds[i],
                self.current_speeds[i],
                dt
            )
            pwm_values.append(pwm)

        return pwm_values

    def update_encoder_feedback(self, encoder_counts):
        """更新编码器反馈"""
        # 将编码器计数转换为转速
        # 假设编码器每转产生N个脉冲
        encoder_cpr = 1000  # 编码器每转脉冲数
        for i in range(4):
            # 简化计算：rpm = (counts_per_second * 60) / cpr
            self.current_speeds[i] = encoder_counts[i] * 60 / encoder_cpr

class SmoothMecanumController:
    def __init__(self):
        # 速度限制
        self.max_linear_accel = 2.0  # 最大线加速度 m/s²
        self.max_angular_accel = 5.0  # 最大角加速度 rad/s²

        # 当前速度
        self.current_vx = 0
        self.current_vy = 0
        self.current_omega = 0

        # 目标速度
        self.target_vx = 0
        self.target_vy = 0
        self.target_omega = 0

    def set_smooth_velocity(self, target_vx, target_vy, target_omega, dt):
        """平滑设置速度，避免急加速"""
        # 计算加速度
        accel_vx = (target_vx - self.current_vx) / dt
        accel_vy = (target_vy - self.current_vy) / dt
        accel_omega = (target_omega - self.current_omega) / dt

        # 限制加速度
        accel_vx = max(-self.max_linear_accel, min(self.max_linear_accel, accel_vx))
        accel_vy = max(-self.max_linear_accel, min(self.max_linear_accel, accel_vy))
        accel_omega = max(-self.max_angular_accel, min(self.max_angular_accel, accel_omega))

        # 更新速度
        self.current_vx += accel_vx * dt
        self.current_vy += accel_vy * dt
        self.current_omega += accel_omega * dt

        return self.current_vx, self.current_vy, self.current_omega

class TrajectoryTracker:
    def __init__(self):
        # 位姿
        self.x = 0  # 当前位置x
        self.y = 0  # 当前位置y
        self.theta = 0  # 当前角度

        # 控制参数
        self.k_linear = 2.0  # 线速度增益
        self.k_angular = 3.0  # 角速度增益

    def follow_trajectory(self, target_x, target_y, target_theta=None):
        """
        跟踪轨迹到目标点
        """
        # 计算位置误差
        dx = target_x - self.x
        dy = target_y - self.y

        # 计算距离和角度误差
        distance = math.sqrt(dx**2 + dy**2)

        if distance < 0.01:  # 到达目标点
            return 0, 0, 0

        # 计算目标方向
        target_heading = math.atan2(dy, dx)

        # 角度误差（归一化到[-π, π]）
        heading_error = target_heading - self.theta
        while heading_error > math.pi:
            heading_error -= 2 * math.pi
        while heading_error < -math.pi:
            heading_error += 2 * math.pi

        # 如果指定了目标角度，还要考虑最终角度误差
        final_angle_error = 0
        if target_theta is not None:
            final_angle_error = target_theta - self.theta
            while final_angle_error > math.pi:
                final_angle_error -= 2 * math.pi
            while final_angle_error < -math.pi:
                final_angle_error += 2 * math.pi

        # 控制律
        # 线速度：距离越近速度越慢
        linear_vel = min(0.5, distance * self.k_linear)

        # 角速度：转向目标方向
        angular_vel = heading_error * self.k_angular

        # 接近目标时，如果需要，调整最终角度
        if distance < 0.1 and target_theta is not None:
            angular_vel = final_angle_error * self.k_angular
            linear_vel = 0

        # 将线速度分解为vx, vy
        vx = linear_vel * math.cos(self.theta)
        vy = linear_vel * math.sin(self.theta)

        return vx, vy, angular_vel