while True:
pygame.event.clear()
# get message from robotConnection
message = robotConn.readMessage(simulate=True)
if message is None:
pygame.event.pump()
clock.tick(20)
continue
# make robot motion
if message.type == "Left":
robot.motion(message)
# get sample points after motion
robot.updateMap(message, map.update)
elif message.type == "Corner":
robot.updateCorner(message, map.update)
else:
print("Error message type")
continue
# get sample points after motion
robot.updateMap(message, map.update)
# Draw Robot
print(robot.pose)
sx, sy, lx, ly, rx, ry = robot.getRobotShape(robot.pose)
pygame.draw.polygon(screen,(255,0,0),((rx,ry),(lx,ly),(sx,sy)),2)
# Draw map
points = []
points = map.getPoints()
for point in points:
pygame.draw.circle(screen,(0,0,0),point,2)
pygame.display.flip()
