麦克纳姆轮底盘运动控制实例

import time
import math
from dataclasses import dataclass

@dataclass
class RobotState:
    """机器人状态"""
    x: float = 0  # 位置x
    y: float = 0  # 位置y
    theta: float = 0  # 朝向角度
    vx: float = 0  # x方向速度
    vy: float = 0  # y方向速度
    omega: float = 0  # 角速度

class MecanumRobot:
    def __init__(self):
        # 初始化控制器
        self.kinematics = MecanumKinematics()
        self.base_controller = MecanumBaseController()
        self.trajectory_tracker = TrajectoryTracker()
        self.smooth_controller = SmoothMecanumController()

        # 机器人状态
        self.state = RobotState()

        # 控制周期
        self.control_dt = 0.02  # 50Hz控制频率

        # 运行标志
        self.running = False

    def start(self):
        """启动控制线程"""
        self.running = True
        # 这里可以启动控制线程
        # threading.Thread(target=self.control_loop).start()

    def control_loop(self):
        """主控制循环"""
        last_time = time.time()

        while self.running:
            current_time = time.time()
            dt = current_time - last_time

            if dt >= self.control_dt:
                # 1. 获取目标速度（从遥控器或轨迹跟踪器）
                vx, vy, omega = self.get_target_velocity()

                # 2. 平滑速度处理
                vx, vy, omega = self.smooth_controller.set_smooth_velocity(
                    vx, vy, omega, dt
                )

                # 3. 设置底盘速度
                self.base_controller.set_chassis_velocity(vx, vy, omega)

                # 4. 执行电机控制
                pwm_values = self.base_controller.update(dt)

                # 5. 发送PWM到电机
                self.set_motor_pwm(pwm_values)

                # 6. 更新里程计
                self.update_odometry(dt)

                last_time = current_time

            time.sleep(0.001)  # 短暂休眠

    def get_target_velocity(self):
        """获取目标速度（根据应用场景实现）"""
        # 示例：可以从遥控器、轨迹跟踪器或自主决策获取
        # 这里返回固定值作为示例
        return 0.2, 0.1, 0.5  # vx, vy, omega

    def set_motor_pwm(self, pwm_values):
        """设置电机PWM（需要根据硬件实现）"""
        # 伪代码：设置四个电机的PWM
        # 实际中可能是GPIO、CAN总线或串口通信
        pass

    def update_odometry(self, dt):
        """更新里程计"""
        # 从编码器获取轮子转速
        # 假设我们有编码器读数
        encoder_counts = [0, 0, 0, 0]  # 需要实际获取
        self.base_controller.update_encoder_feedback(encoder_counts)

        # 使用正运动学计算机器人速度
        w1, w2, w3, w4 = self.base_controller.current_speeds
        # 转换为弧度/秒
        w_rad = [w * 2 * math.pi / 60 for w in [w1, w2, w3, w4]]

        # 计算底盘速度
        vx, vy, omega = self.kinematics.wheel_to_chassis(*w_rad)

        # 更新机器人状态
        self.state.vx = vx
        self.state.vy = vy
        self.state.omega = omega

        # 更新位置
        self.state.x += (vx * math.cos(self.state.theta) - vy * math.sin(self.state.theta)) * dt
        self.state.y += (vx * math.sin(self.state.theta) + vy * math.cos(self.state.theta)) * dt
        self.state.theta += omega * dt

        # 角度归一化
        self.state.theta = math.atan2(math.sin(self.state.theta), math.cos(self.state.theta))

    def move_to_point(self, target_x, target_y, target_theta=None):
        """移动到指定点"""
        while True:
            # 获取控制量
            vx, vy, omega = self.trajectory_tracker.follow_trajectory(
                target_x, target_y, target_theta
            )

            # 设置速度
            self.base_controller.set_chassis_velocity(vx, vy, omega)

            # 检查是否到达
            dx = target_x - self.state.x
            dy = target_y - self.state.y
            distance = math.sqrt(dx**2 + dy**2)

            if distance < 0.02:  # 2cm容差
                break

            time.sleep(self.control_dt)

# 使用示例
if __name__ == "__main__":
    robot = MecanumRobot()

    # 示例1：简单移动
    print("向前移动")
    robot.base_controller.set_chassis_velocity(0, 0.5, 0)

    # 示例2：斜向移动
    print("右前方45度移动")
    vx, vy, _ = MecanumModes.move_diagonal(0.5, 45)
    robot.base_controller.set_chassis_velocity(vx, vy, 0)

    # 示例3：原地旋转
    print("原地旋转")
    robot.base_controller.set_chassis_velocity(0, 0, 1.0)

    # 示例4：轨迹跟踪
    print("移动到点(1, 1)")
    robot.move_to_point(1.0, 1.0)