差速轮组的开环及闭环控制算法

class OpenLoopController:
    def __init__(self):
        self.left_pwm = 0
        self.right_pwm = 0

    def set_velocity(self, v, omega):
        """设置底盘线速度和角速度"""
        # 计算期望轮速
        v_left, v_right = chassis_to_wheel(v, omega)

        # 转换为轮子转速（RPM）
        rpm_left = (v_left / (2 * 3.14 * wheel_radius)) * 60
        rpm_right = (v_right / (2 * 3.14 * wheel_radius)) * 60

        # 转换为PWM值
        self.left_pwm = rpm_to_pwm(rpm_left)
        self.right_pwm = rpm_to_pwm(rpm_right)

        return self.left_pwm, self.right_pwm

class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp        # 比例系数
        self.ki = ki        # 积分系数
        self.kd = kd        # 微分系数
        self.prev_error = 0
        self.integral = 0

    def update(self, target, current, dt):
        """dt: 时间间隔（秒）"""
        error = target - current

        # 比例项
        p = self.kp * error

        # 积分项（防止积分饱和）
        self.integral += error * dt
        self.integral = max(-100, min(100, self.integral))  # 限幅
        i = self.ki * self.integral

        # 微分项
        d = self.kd * (error - self.prev_error) / dt
        self.prev_error = error

        return p + i + d

class DriveController:
    def __init__(self):
        # 为每个轮子创建PID控制器
        self.pid_left = PIDController(kp=1.0, ki=0.1, kd=0.05)
        self.pid_right = PIDController(kp=1.0, ki=0.1, kd=0.05)

        # 编码器反馈
        self.left_encoder = 0
        self.right_encoder = 0

    def update(self, v_target, omega_target, dt):
        """主控制循环"""
        # 1. 计算目标轮速
        v_left_target, v_right_target = chassis_to_wheel(v_target, omega_target)

        # 2. 获取当前轮速（从编码器）
        v_left_current = self.get_left_speed(dt)
        v_right_current = self.get_right_speed(dt)

        # 3. PID计算PWM调整量
        pwm_left_adjust = self.pid_left.update(v_left_target, v_left_current, dt)
        pwm_right_adjust = self.pid_right.update(v_right_target, v_right_current, dt)

        # 4. 基础PWM + 调整量
        base_pwm = 100  # 基础值
        pwm_left = base_pwm + pwm_left_adjust
        pwm_right = base_pwm + pwm_right_adjust

        return pwm_left, pwm_right

    def get_left_speed(self, dt):
        """从编码器计算实际速度"""
        # 根据编码器脉冲数和时间计算速度
        return 0  # 返回实际速度值

    def get_right_speed(self, dt):
        return 0  # 返回实际速度值

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

    def move_to_point(self, target_x, target_y, max_speed=0.5):
        """移动到目标点"""
        # 计算距离和角度差
        dx = target_x - self.x
        dy = target_y - self.y
        distance = math.sqrt(dx*dx + dy*dy)

        # 目标角度
        target_angle = math.atan2(dy, dx)
        angle_error = target_angle - self.theta

        # 角度归一化到[-pi, pi]
        while angle_error > math.pi:
            angle_error -= 2*math.pi
        while angle_error < -math.pi:
            angle_error += 2*math.pi

        # 控制策略
        if distance < 0.05:  # 到达目标
            v = 0
            omega = 0
        else:
            # 线速度：距离越近速度越慢
            v = min(max_speed, distance * 0.5)

            # 角速度：转向目标角度
            omega = angle_error * 2.0

            # 限制最大角速度
            omega = max(-1.0, min(1.0, omega))

        return v, omega