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('','')
