def findDirectionToGo(self, realHeading, destHeading):
diffAngle = min(abs(destHeading - realHeading), 360 - abs(realHeading - destHeading))
print "find direction to go... real:", realHeading, "expect:", destHeading, "diff:", diffAngle,"mega:",self.curHeading,self.offsetDirection,self.curHeading+self.offsetDirection
if (diffAngle < self.ANGLE_LIMIT) :
print "Go straight"
if (AudioManager.isBusy() == False):
AudioManager.play("straight_ahead")
elif (realHeading + diffAngle + 360) % 360 == destHeading:
print "Turn right " + str(realHeading) + " " + str((realHeading + diffAngle + 360) % 360) + " " + str(destHeading)
if (AudioManager.isBusy() == False) :
AudioManager.play("right")
else :
print "Turn left " + str(realHeading) + " " + str((realHeading - diffAngle + 360) % 360) + " " + str(destHeading)
if (AudioManager.isBusy() == False) :
AudioManager.play("left")
def getInstruction(self):
if len(self.pathToGo) <= 1:
print "No more node!"
return
if self.hasReachedDestination:
return
if self.hasUpdate:
self.calculateDistanceToDestination(self.mapManager.getNode(self.pathToGo[1][0], self.pathToGo[1][1], self.pathToGo[1][2]))
self.hasUpdate = False
realHeading = (self.mapHeading + self.curHeading + self.offsetDirection) % 360
#realHeading = (self.mapHeading + self.curHeading) % 360
#print realHeading
curX = int(self.curX / self.GRID_LENGTH)
curY = int(self.curY / self.GRID_LENGTH)
# if cannot reach destination,
if self.minDist[curX][curY] == self.INF:
self.isCalculating = True
print "Path is blocked! Finding another path..."
self.mapManager.blockPath(self.pathToGo[0][0], self.pathToGo[0][1], self.pathToGo[0][2], self.pathToGo[1][2])
self.pathToGo = self.dijkstra(self.mapManager.getNode(self.pathToGo[0][0], self.pathToGo[0][1], self.pathToGo[0][2]), self.endNode)
if (len(self.pathToGo) <= 1) or (self.pathToGo[-1] != self.endNode):
print "You will never reach the destination!!!"
return
self.calculateDistanceToDestination(self.mapManager.getNode(self.pathToGo[0][0], self.pathToGo[0][1], self.pathToGo[0][2]))
self.isCalculating = False
#print self.minDist[curX][curY]
if (self.map[curX][curY] == self.pathToGo[1][2]):
self.preStepCount = self.stepCount
self.stepCount = 0
print 'You have reached node' ,self.map[curX][curY]
audioName = str(self.curBuilding) + "-" + str(self.curLevel) + "-" + str(self.map[curX][curY])
print audioName
AudioManager.play(audioName)
#AudioManager.playNumber(self.map[curX][curY])
self.isCalculating = True
self.prepareRouteToNextPoint()
curNode = self.mapManager.getNode(self.pathToGo[0][0], self.pathToGo[0][1], self.pathToGo[0][2])
self.offsetDirection = curNode.offset
if len(self.pathToGo) <= 1:
print "You reached the destination!"
AudioManager.play('destination_reached')
self.hasReachedDestination = True
return
self.calculateDistanceToDestination(self.mapManager.getNode(self.pathToGo[1][0], self.pathToGo[1][1], self.pathToGo[1][2]))
self.isCalculating = False
self.printMap()
possibleHeading = []
for i in range(0, 8):
v = (curX + self.nextDir[i][0], curY + self.nextDir[i][1])
#print v, self.minDist[v[0]][v[1]], self.minDist[curX][curY]
if (self.isWall(v[0], v[1]) == False) and (self.minDist[v[0]][v[1]] < self.minDist[curX][curY]):
#possibleHeading.append(self.nextDir[i][2])
possibleHeading.append(i)
if len(possibleHeading) > 0:
chosenHeading = self.chooseHeading(possibleHeading, curX, curY, realHeading)
self.findDirectionToGo(realHeading, chosenHeading)
else:
print "Cannot find any direction! Try to turn right..."
if (AudioManager.isBusy() == False) :
AudioManager.play("right")
