差速底盘运动控制程序实例

import time
import math

class Robot:
    def __init__(self):
        # 底盘参数
        self.wheel_radius = 0.05    # 轮子半径（米）
        self.wheel_distance = 0.30      # 两轮间距（米）

        # PID参数（需要根据实际调整）
        self.kp = 1.5
        self.ki = 0.2
        self.kd = 0.1

        # 状态变量
        self.left_speed = 0
        self.right_speed = 0
        self.x = 0
        self.y = 0
        self.theta = 0

        # 初始化控制器
        self.controller = DriveController()

    def set_target_velocity(self, linear, angular):
        """设置目标线速度和角速度"""
        dt = 0.01  # 控制周期，10ms

        # 获取控制量
        pwm_left, pwm_right = self.controller.update(
            linear, angular, dt
        )

        # 发送PWM信号到电机
        self.set_motor_pwm(pwm_left, pwm_right)

        # 更新位姿
        self.update_odometry(dt)

    def set_motor_pwm(self, left_pwm, right_pwm):
        """实际控制电机（根据硬件控制器实现）"""
        # 设置PWM输出
        # GPIO输出或电机驱动器控制
        pass

    def update_odometry(self, dt):
        """更新里程计（位置估计）"""
        # 从编码器获取实际轮速
        left_actual = self.get_left_speed()
        right_actual = self.get_right_speed()

        # 计算底盘速度
        v = (right_actual + left_actual) / 2
        omega = (right_actual - left_actual) / self.wheel_distance

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

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

    def run_trajectory(self, waypoints):
        """运行轨迹跟踪"""
        for (target_x, target_y) in waypoints:
            while True:
                # 计算控制量
                v, omega = self.trajectory_controller.move_to_point(
                    target_x, target_y
                )

                # 执行控制
                self.set_target_velocity(v, omega)

                # 检查是否到达
                dx = target_x - self.x
                dy = target_y - self.y
                if math.sqrt(dx*dx + dy*dy) < 0.02:  # 2cm容差
                    break

                time.sleep(0.01)  # 控制频率100Hz

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

    # 示例1：简单速度控制
    robot.set_target_velocity(0.5, 0)  # 以0.5m/s直行

    # 示例2：原地旋转
    robot.set_target_velocity(0, 1.0)  # 原地旋转

    # 示例3：轨迹跟踪
    waypoints = [(1, 0), (1, 1), (0, 1), (0, 0)]  # 矩形轨迹
    robot.run_trajectory(waypoints)