deltay = int(robot.fy - robot.y) print(robot.fx, robot.fy, robot.x, robot.y) print(deltax, deltay) print(cluster_direction) if deltax == 0 and deltay == 0: if len(cluster_direction) > 0: step = cluster_direction[len(cluster_direction) - 1] cluster_direction.pop() angle = delta_degree[step[i][0]] old_length = length length += len(step) # print coordinate_path[length] gridworld = GridWorld(screen, robot, cell_width, cell_height, cell_margin, init, goal, grid) smooth_path = gridworld.smooth_path(path) gridworld.draw_path(smooth_path) robot.set(coordinate_path[old_length][0], coordinate_path[old_length][1], angle) pygame.draw.circle(screen, yellow, [ coordinate_path[old_length][0], coordinate_path[old_length][1] ], 5) robot.destination(coordinate_path[length][0], coordinate_path[length][1]) pygame.draw.circle( screen, green, [coordinate_path[length][0], coordinate_path[length][1]], 5) gridworld.show() print(angle, length)