def test(): initialize() waypointNav.addWaypoints([[0, 0, math.radians(135), True], [0, 0, 0, True]]) while True: start = time.time() update() sleep(max(0, 1.0 / 50.0 - time.time() - start))
def goMap(): global myMap myMap.initMapping() initialize() pygame.init() fpsClock = pygame.time.Clock() width,height = 900,900 main_surface = pygame.display.set_mode((width, height)) pygame.display.set_caption('Robot location and vision') RED_COLOR = pygame.Color(255,0,0) WHITE_COLOR = pygame.Color(255,255,255) BLACK_COLOR = pygame.Color(0,0,0) BLUE_COLOR = pygame.Color(0,0,255) realHeight,realWidth = 900,900 ROBOT_RADIUS = 7 p = CPosition() q = False i = 0 waypointNav.addWaypoint([450,450,0,False]) while not q: update() #print waypointNav.wp myMap.robotPositioned(ctypes.c_double(pose[0]), ctypes.c_double(pose[1])) for sen in sensorPoints: if(sen[2]): myMap.wallDetected(ctypes.c_double(sen[0]), ctypes.c_double(sen[1])) else: myMap.wallNotDetected(ctypes.c_double(sen[0]), ctypes.c_double(sen[1])) if len(waypointNav.wp) == 0: rotate() myMap.setConfigSpace() myMap.closestUnvisited(ctypes.byref(p)) myMap.goPlan(ctypes.c_double(p.x), ctypes.c_double(p.y)) waypoints = [] for j in xrange(myMap.getPlanLength()): myMap.getPlanWP(j,ctypes.byref(p)) waypoints.append([p.x,p.y,0,False]) print waypoints waypointNav.addWaypoints(waypoints) mw,mh = main_surface.get_size() main_surface.fill(WHITE_COLOR) myMap.setConfigSpace() for x in xrange(realHeight): for y in xrange(realWidth): if(myMap.getWall(x,y)): pygame.draw.rect(main_surface,BLACK_COLOR,(x,(realHeight-y),3,3)) pygame.draw.circle(main_surface, BLUE_COLOR, (int(pose[0]),(realHeight-int(pose[1]))), ROBOT_RADIUS, 3) print "Here" for s in sensorPoints: if(s[2]): color = RED_COLOR else: color = BLACK_COLOR pygame.draw.line(main_surface, color, (int(pose[0]),(realHeight-int(pose[1]))), (int(s[0]),(realHeight-int(s[1]))), 3) for event in pygame.event.get(): if event.type == QUIT: pygame.quit() q = True if not q: pygame.display.update() fpsClock.tick(120) cleanQuit('','')