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)