Exemple #1
0
def goMapping(freq=30):
    global myMap,q

    # Parameters
    RED_BALL = 4
    GREEN_BALL = 5

    myMap.initMapping()
    while not q:
        start = time.time()
        sensorPoints = ct.getSensorPoints()
        pose = ct.getPose()

        balls = [x for x in v.getAreas(pose) if v.isBall(x)]
        knownBalls = []
        for b in balls:
            bc = v.getBallCoords(b,pose)
            if bc is not None:
                knownBalls.append(bc)

        # Mapping update
        myMap.robotPositioned(ctypes.c_double(pose[0]), ctypes.c_double(pose[1]))
        for a in knownBalls:
            myMap.ballDetected(ctypes.c_double(a[0]),ctypes.c_double(a[1]),ctypes.c_int(RED_BALL))
        for s in sensorPoints:
            if(s[2]):
                myMap.wallDetected(ctypes.c_double(s[0]), ctypes.c_double(s[1]))
            else:
                myMap.wallNotDetected(ctypes.c_double(s[0]), ctypes.c_double(s[1]))
        time.sleep(max(0,1.0/float(freq) - (time.time()-start)))
Exemple #2
0
def goPygame():
    global myMap,q,recordedBalls
    pygame.init()

    # Parameters
    width,height = 900,900
    realWidth,realHeight = 450,450
    ROBOT_RADIUS = 7
    BALL_RADIUS = 2.5/2.0
    RED_COLOR = pygame.Color(255,0,0)
    GREEN_COLOR = pygame.Color(0,255,0)
    WHITE_COLOR = pygame.Color(255,255,255)
    BLACK_COLOR = pygame.Color(0,0,0)
    BLUE_COLOR = pygame.Color(0,0,255)
    prop = int((width/realWidth + height/realHeight)/2)

    fpsClock = pygame.time.Clock()
    pygame.display.set_caption('Robot location and vision')
    main_surface = pygame.display.set_mode((width, height))
    fieldMap = pygame.transform.scale(
        pygame.image.load("simulator/map.bmp"),
        (width,height))
    main_surface.fill(WHITE_COLOR)
    #main_surface.blit(fieldMap, (0,0))

    while not q:
        pose = ct.getPose()
        sensorPoints = ct.getSensorPoints()
        #main_surface.blit(fieldMap, (0,0))
        main_surface.fill(WHITE_COLOR)

        #balls = v.getAreas(pose)
        #for b in balls:
        #    pygame.draw.circle(main_surface, BLUE_COLOR,
        #                       (int(b[0])*prop, (realHeight-1-int(b[1]))*prop),
        #                       int(BALL_RADIUS)*prop, 0)

        pygame.draw.circle(main_surface, BLUE_COLOR,
                           (int(pose[0])*prop,(realHeight-1-int(pose[1]))*prop),
                           ROBOT_RADIUS*prop, prop)
        for s in sensorPoints:
            if(s[2]):
                color = RED_COLOR
            else:
                color = BLACK_COLOR
            pygame.draw.line(main_surface, color,
                             (int(pose[0])*prop,(realHeight-1-int(pose[1]))*prop),
                             (int(s[0])*prop,(realHeight-1-int(s[1]))*prop),
                             prop)
        for x in xrange(realWidth):
            for y in xrange(realHeight):
                if(myMap.getWall(x,y)):
                    pygame.draw.rect(main_surface, RED_COLOR,(x*prop,(realHeight-1-y)*prop,prop,prop))
                elif(myMap.getBall(x,y)):
                    pygame.draw.circle(main_surface, BLUE_COLOR,
                                       (x*prop, (realHeight-1-y)*prop),
                                       int(BALL_RADIUS)*prop, 0)

        for event in pygame.event.get():
            if event.type == QUIT:
                pygame.quit()
                q = True
        if not q:
            pygame.display.update()
        fpsClock.tick(50)