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)
