def run(): IMU = arduino.IMU() start = (0, 0) end = (200, 200) grid, start, end = mapmaker.read() mToI = 1 posX = buggyinfo.Position(start[0], metersToIndex=mToI) posY = buggyinfo.Position(start[1], metersToIndex=mToI) posZ = buggyinfo.Position(0, initialV=1, metersToIndex=mToI) drawer = draw.Drawer(300, 300, 700, 700, start, end) while drawer.done is False: accel, gyro, dt = IMU.getIMU() accel.x = 0.01 accel.y = 0.01 dt = 1 posX.update(accel.x, dt) posY.update(accel.y, dt) posZ.update(accel.z, dt) drawer.updateNode(posX.convert(), posY.convert(), posZ.convert(), False) drawer.loop() drawer.drawParticle(int(posX), int(posY)) drawer.update()
def loop(self): self.screen.fill(Drawer.GRAY) for event in pygame.event.get(): if event.type == pygame.QUIT: self.done = True break elif event.type == pygame.MOUSEBUTTONDOWN: self.mouseDown = True gridX, gridY = self.getCellIndex() # set the brush mode to the opposite of the selected cell self.brushMode = not self.grid[gridY, gridX, 0] elif event.type == pygame.MOUSEBUTTONUP: self.mouseDown = False elif event.type == pygame.MOUSEMOTION: self.offset = list(self.getCellIndex()) self.offset[0] -= self.window[0] / 2 self.offset[1] -= self.window[1] / 2 if self.offset[0] + self.window[0] >= self.gridWidth: self.offset[0] = self.gridWidth - self.window[0] if self.offset[1] + self.window[1] >= self.gridHeight: self.offset[1] = self.gridHeight - self.window[1] if self.offset[0] < 0: self.offset[0] = 0 if self.offset[1] < 0: self.offset[1] = 0 self.updateVisible() elif event.type == pygame.KEYDOWN: if event.key == pygame.K_SPACE: if len(self.waypoints) == 0: self.path, self.gridInfo, plotTime = astar.search( self.grid, self.start, self.end) # find a path between the start and end else: self.path, self.gridInfo, plotTime = astar.searchWaypoints( self.grid, [self.start] + self.waypoints + [self.end] ) # find a path between the start, waypoints, and the end print "a* time:", plotTime # change the brush size (only squares) elif event.key == pygame.K_EQUALS: self.brushSize += 1 print(self.brushSize) elif event.key == pygame.K_MINUS: self.brushSize -= 1 if self.brushSize < 1: self.brushSize = 1 print(self.brushSize) # if key is a numbered key, change brush size to that key elif 0 <= event.key - ord('0') <= 9: self.brushSize = int(event.key - ord('0')) if self.brushSize < 1: self.brushSize = 1 print(self.brushSize) # set start position to the position of the mouse elif event.key == pygame.K_s: self.start = self.getCellIndex() self.grid[self.start[1], self.start[0], 0] = 0 # set end position to the position of the mouse elif event.key == pygame.K_e: self.end = self.getCellIndex() self.grid[self.end[1], self.end[0], 0] = 0 elif event.key == pygame.K_ESCAPE or event.key == pygame.K_q: self.done = True break # change the visible window by self.delOffset depending on the direction of arrow keys # elif pygame.K_UP <= event.key <= pygame.K_LEFT: # if event.key == pygame.K_UP: # self.offset[1] -= self.delOffset[1] # if self.offset[1] < 0: # self.offset[1] = 0 # elif event.key == pygame.K_DOWN: # self.offset[1] += self.delOffset[1] # if self.offset[1] + self.window[1] >= self.gridHeight: # self.offset[1] = self.gridHeight - self.window[1] # elif event.key == pygame.K_LEFT: # self.offset[0] -= self.delOffset[0] # if self.offset[0] < 0: # self.offset[0] = 0 # elif event.key == pygame.K_RIGHT: # self.offset[0] += self.delOffset[0] # if self.offset[0] + self.window[0] >= self.gridWidth: # self.offset[0] = self.gridWidth - self.window[0] # self.updateVisible() # Fill the entire grid elif event.key == pygame.K_f: self.grid = numpy.ones( (self.gridHeight, self.gridWidth, 2)) self.grid[:, :, 1] = 0 self.grid[self.start[1], self.start[0], 0] = 0 self.grid[self.end[1], self.end[0], 0] = 0 self.updateVisible() # reveal the entire grid elif event.key == pygame.K_TAB: self.showFull = True # reset grid to initial state elif event.key == pygame.K_c: self.grid = numpy.zeros( (self.gridHeight, self.gridWidth, 2)) self.path = [] self.waypoints = [] self.gridInfo = {} self.updateVisible() # add a waypoint to current mouse position elif event.key == pygame.K_w: cellX, cellY = self.getCellIndex() self.waypoints.append((cellX, cellY)) # delete waypoint nearest to mouse elif event.key == 8: # delete key cellX, cellY = self.getCellIndex() closestPoint = 0 shorestDist = None for index, (wpY, wpX) in enumerate(self.waypoints): distance = Drawer.pythagoreanDist( wpX, wpY, cellX, cellY) if distance < shorestDist or shorestDist is None: shorestDist = distance closestPoint = index print self.waypoints.pop(closestPoint) # save map data to text file or load map data from text file if map is empty elif event.key == pygame.K_RETURN: if numpy.all(self.grid[:, :, 0] == 0): self.grid, self.start, self.end = mapmaker.read() self.initGridDisplay(self.grid.shape[1], self.grid.shape[0]) self.updateVisible() else: mapmaker.write(self.grid, self.start, self.end) elif pygame.K_i <= event.key <= pygame.K_l: if event.key == pygame.K_i: self.particleAccel[1] -= 1 elif event.key == pygame.K_j: self.particleAccel[0] -= 1 elif event.key == pygame.K_k: self.particleAccel[1] += 1 elif event.key == pygame.K_l: self.particleAccel[0] += 1 elif event.type == pygame.KEYUP: if event.key == pygame.K_TAB: self.showFull = False if self.mouseDown == True: clickedX, clickedY = self.getCellIndex() radius = int(round(float(self.brushSize) / 2)) for gridY in xrange( max(clickedY - radius + self.brushSize % 2, 0), min(clickedY + radius, self.gridHeight)): for gridX in xrange( max(clickedX - radius + self.brushSize % 2, 0), min(clickedX + radius, self.gridWidth)): self.grid[gridY, gridX, 0] = self.brushMode self.updateVisible() for (cellY, cellX), element in self.enumerateGrid(): if self.showFull == False: cellX += self.offset[0] cellY += self.offset[1] color = Drawer.WHITE if element == 1: color = Drawer.GREEN self.drawCell(cellX, cellY, color) self.drawCell(self.start[0], self.start[1], Drawer.RED) self.drawCell(self.end[0], self.end[1], Drawer.BLUE) for (cellX, cellY) in self.waypoints: self.drawCell(cellX, cellY, Drawer.BLACK) for (cellY, cellX), element in self.enumerateGrid(): if self.showFull == False: cellX += self.offset[0] cellY += self.offset[1] if len(self.gridInfo) > 0: parent = self.gridInfo[(cellX, cellY)].parent if parent is not None: self.connectCells(cellX, cellY, parent[0], parent[1], Drawer.RED, 1) for index in xrange(1, len(self.path)): self.connectCells(self.path[index - 1][0], self.path[index - 1][1], self.path[index][0], self.path[index][1]) for (x, y) in zip(self.bezierXvals, self.bezierYvals): self.screen.fill(Drawer.BLUE, ((self.cellSize * (x + 0.5), self.cellSize * (y + 0.5)), (1, 1))) pygame.draw.rect(self.screen, Drawer.BLACK, [ 0, 0, self.cellSize * self.gridWidth, self.cellSize * self.gridHeight ], 1)