Python实现Micromouse运动指令转坐标 适配pygame显示与A*寻路

原代码逻辑问题 #

原坐标计算逻辑存在3个核心错误，导致输出坐标不准：

• 定义的current变量用于标记当前移动轴，但全程未更新，默认值始终为"y"，x轴移动逻辑永远不会正确触发
• 角度值未做归一化处理，多次转向后角度数值会超过360或低于0，仅通过>=90/<=90判断方向完全失效
• 未建立角度与x/y增量的明确映射关系，方向判断逻辑混乱

正确实现思路 #

采用角度归一化+方向增量映射的方案，输出的坐标点既适配pygame屏幕坐标系，也可直接作为A*算法的网格节点输入：

• 初始状态设定：默认电脑鼠初始朝向为正上方（对应pygame坐标系y值减小方向，因为pygame原点在屏幕左上角，y轴向下），初始角度记为0
• 转向处理：遇到L90则角度加90，遇到R90则角度减90，每次转向后对角度做%360运算，保证角度始终落在0-359的合法区间
• 移动处理：遇到FD指令时，根据当前角度对应固定坐标增量计算新位置：
  • 角度0（朝上）：x不变，y减10
  • 角度90（朝左）：x减10，y不变
  • 角度180（朝下）：x不变，y加10
  • 角度270（朝右）：x加10，y不变
• 每次移动后将新坐标存入路径列表，所有坐标点步长统一为10，属于规则网格点，可直接传入A*算法做节点计算。如果不需要pygame的屏幕居中偏移，只需将初始坐标从(150,150)改为(0,0)即可。

修复后完整代码 #

import pygame

# 板载控制器输出的原始指令列表
raw_commands = ["FD","FD","R90","FD","FD","FD","FD","FD","FD","L90","FD","FD","FD"]

# 窗口配置
(width, height) = (300, 300)
background_colour = (255,255,255)

# 初始化pygame窗口
screen = pygame.display.set_mode((width, height))
pygame.display.set_caption('Micromouse Tracer')
screen.fill(background_colour)

def draw(start, end):
    pygame.draw.line(screen, (0,0,0), start, end, 5)

def convert(commands):
    calcd_list = []
    for cmd in commands:
        if cmd == "FD":
            calcd_list.append(10)
        elif cmd == "R90":
            calcd_list.append(-90)
        elif cmd == "L90":
            calcd_list.append(90)
    return calcd_list

def makepath(commands, start_pos=(150,150)):
    path_list = convert(commands)
    x, y = start_pos
    path = [(x,y)]
    current_degree = 0  # 初始朝向：正上方

    for item in path_list:
        if item == 10:
            # 根据当前角度计算移动增量
            if current_degree == 0: # 朝上
                y -= 10
            elif current_degree == 90: # 朝左
                x -= 10
            elif current_degree == 180: # 朝下
                y += 10
            elif current_degree == 270: # 朝右
                x += 10
            path.append((x,y))
        else:
            # 更新角度并归一化到0-359区间
            current_degree = (current_degree + item) % 360
    return path

# 生成路径并绘制
path_points = makepath(raw_commands)
print(path_points)
for i in range(len(path_points)):
    if i > 0:
        draw(path_points[i-1], path_points[i])

pygame.display.flip()

# 窗口退出逻辑
running = True
while running:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False

输出验证 #

针对测试指令序列，输出的坐标序列如下，轨迹绘制和A*节点输入均可直接使用：
[(150, 150), (150, 140), (150, 130), (160, 130), (170, 130), (180, 130), (190, 130), (200, 130), (210, 130), (210, 120), (210, 110), (210, 100)]

内容的提问来源于stack exchange，提问作者Jack Fitton