def calibrateStep(voiceOutput):
# voiceOutput = Voice()
fileManager = Storage()
serial = SerialCommunicator()
data_path = fileManager.getFilePath('data', 'calibration_info.txt')
data_exist = fileManager.readFromFile(data_path)
if data_exist is None:
# voiceOutput.say(messages.CALIBRATION_START)
voiceOutput.addToQueue(messages.CALIBRATION_START, constants.LOW_PRIORITY)
serial.serialWrite('c')
calibration_info= serial.serialRead()
print "from serial",
print calibration_info
calibration_info_splited = calibration_info.split(' ')
calibration_info_filtered = []
for i in range(len(calibration_info_splited)):
if calibration_info_splited[i] != "":
calibration_info_filtered.append(calibration_info_splited[i])
constants.STEP_LENGTH = float(calibration_info_filtered[0])
constants.WALKING_DEGREE_ERROR = float(calibration_info_filtered[1])
constants.IR_STAIRS_CONSTANT = float(calibration_info_filtered[2])
constants.PEAK_ACC_VALUE = float(calibration_info_filtered[3])
print "step size is " ,
print constants.STEP_LENGTH,
print " m"
print "orientation degree error is " ,
print constants.ORIENTATION_DEGREE_ERROR ,
print " degrees"
print "IR stairs constant is ",
print constants.IR_STAIRS_CONSTANT,
print " m"
print constants.PEAK_ACC_VALUE,
print " m/s^2"
# voiceOutput.say(messages.CALIBRATION_END)
voiceOutput.addToQueue(messages.CALIBRATION_END, constants.LOW_PRIORITY)
fileManager.writeListToFile(data_path, calibration_info_filtered)
else:
#ser.write('2') #remove after inte with mega
#ser.readline() #remove after inte with mega
serial.serialWrite('n')
constants.STEP_LENGTH = float((fileManager.readFileToList(data_path))[0])
constants.WALKING_DEGREE_ERROR = float((fileManager.readFileToList(data_path))[1])
constants.IR_STAIRS_CONSTANT = float((fileManager.readFileToList(data_path))[2])
constants.PEAK_ACC_VALUE = float((fileManager.readFileToList(data_path))[3])
serial.serialWrite(str(constants.PEAK_ACC_VALUE))
dataReceived = serial.serialRead()
dataSplited = dataReceived.split(' ')
dataFiltered = []
for i in range(len(dataSplited)):
if dataSplited[i] != "":
dataFiltered.append(dataSplited[i])
constants.COMPASS_OFFSET = float(dataFiltered[0])
constants.YPR_OFFSET = float(dataFiltered[1])
def __init__(self, voice): ### OBJECTS ### self.wifi = Wifi() # self.voiceOutput = Voice() self.serial = SerialCommunicator() self.voiceOutput = voice ### CLASS ATTRIBUTES ### self.prevBearing = 0 self.prevStairSensor = 0 self.stairSensor = 0 self.headSensor = 0 self.bearingFaced = 0 self.stepDetected = 0 self.lastUpdatedTime = 0 self.lastInstructionTime = 0 self.warningMessage = "" self.prevStep = 0 self.prevAngleVoice = 0 self.warningStairsCount = 0 self.lastTurnTime = 0 ### FLAGS ### self.isStairsDetected = False self.isUpStairs = False self.isDownStairs = False self.onPlatform = False self.rightAfterBearingCheck = False # To allow the walking forward instruction to be triggered directly after bearing is check. ### TIMING ATTRIBUTES FOR STEPS ### self.timeSinceLastStep = time.time() - 3 #to ensure that orientation checking will be triggered the first time self.timeSinceNoStep = time.time() ### COUNTERS ### self.stepsOnPlatform = 0
class Guide():
def __init__(self, voice):
### OBJECTS ###
self.wifi = Wifi()
# self.voiceOutput = Voice()
self.serial = SerialCommunicator()
self.voiceOutput = voice
### CLASS ATTRIBUTES ###
self.prevBearing = 0
self.prevStairSensor = 0
self.stairSensor = 0
self.headSensor = 0
self.bearingFaced = 0
self.stepDetected = 0
self.lastUpdatedTime = 0
self.lastInstructionTime = 0
self.warningMessage = ""
self.prevStep = 0
self.prevAngleVoice = 0
self.warningStairsCount = 0
self.lastTurnTime = 0
### FLAGS ###
self.isStairsDetected = False
self.isUpStairs = False
self.isDownStairs = False
self.onPlatform = False
self.rightAfterBearingCheck = False # To allow the walking forward instruction to be triggered directly after bearing is check.
### TIMING ATTRIBUTES FOR STEPS ###
self.timeSinceLastStep = time.time() - 3 #to ensure that orientation checking will be triggered the first time
self.timeSinceNoStep = time.time()
### COUNTERS ###
self.stepsOnPlatform = 0
##########################################
# Functions called by Navigation
##########################################
def updateCoordinates(self, currCoor, north, apNodes, bearingToFace):
self.receiveDataFromArduino()
if self.stepDetected > self.prevStep :
self.timeSinceLastStep = time.time()
else:
self.timeSinceNoStep = time.time()
imuCoor = self.updateIMUCoor(currCoor, north, bearingToFace)
print " current coor is ",
print imuCoor
# wifiCoor = self.wifi.getUserCoordinates(apNodes)
# newCoor = self.estimateCurrentPosition(imuCoor, wifiCoor, north)
# return newCoor
return imuCoor
def warnUser(self, currCoor, mapNorth):
self.warnHeadObstacle()
if mapNorth == 50 and (currCoor[0] <4350) and (currCoor[0] > 3700) and (currCoor[1] > 1800) and (currCoor[1] < 2750):
self.warnStairs()
self.guideAlongStairs()
def userReachedNode(self, node):
message = messages.NODE_REACHED_TEMPLATE.format(node = node['name'])
print message
# self.voiceOutput.say(message,2)
self.voiceOutput.addToQueue(message, constants.HIGH_PRIORITY)
def userNextNode(self, nextNode):
message = messages.NEXT_NODE_TEMPLATE.format(node = nextNode['name'])
print message
self.voiceOutput.addToQueue(message, constants.HIGH_PRIORITY)
def checkBearing(self, bearingToFace, currCoor, nextCoor):
bearingOffset = int(abs(bearingToFace - self.bearingFaced))
angleToTurn = 0
if ((bearingOffset > constants.ORIENTATION_DEGREE_ERROR) and
(abs(self.timeSinceLastStep-self.timeSinceNoStep) > constants.TIME_TO_CHECK_BEARING)):
if bearingToFace < self.bearingFaced:
if bearingOffset > 180 :
message = messages.TURN_TEMPLATE.format(direction = "right", angle = (360 - bearingOffset))
angleToTurn = 360 - bearingOffset
else :
message = messages.TURN_TEMPLATE.format(direction = "left", angle = bearingOffset)
angleToTurn = bearingOffset
else:
if bearingOffset > 180 :
message = messages.TURN_TEMPLATE.format(direction = "left", angle = 360 - bearingOffset)
angleToTurn = 360 - bearingOffset
else :
message = messages.TURN_TEMPLATE.format(direction = "right", angle = bearingOffset)
angleToTurn = bearingOffset
print message
# self.voiceOutput.say(message)
if abs(self.prevAngleVoice - angleToTurn) != 1 and (time.time() - self.lastTurnTime) >= constants.TURN_INSTRUCTION_FREQ:
self.voiceOutput.addToQueue(message, constants.HIGH_PRIORITY)
self.lastTurnTime = time.time()
self.rightAfterBearingCheck = True
self.prevAngleVoice = angleToTurn
else: #guide user to walk straight
if (self.rightAfterBearingCheck == True) or (time.time() - self.lastInstructionTime) >= constants.INSTRUCTIONS_FREQUENCY:
distToNextNode = math.sqrt((nextCoor[0] - currCoor[0]) ** 2 +
(nextCoor[1] - currCoor[1]) ** 2)
stepsToNextNode = int((distToNextNode/100) / (constants.STEP_LENGTH)) #changed dist from cm to meters
message = messages.WALK_FORWARD_TEMPLATE.format(steps = stepsToNextNode)
print message
# self.voiceOutput.say(message)
self.voiceOutput.addToQueue(message, constants.LOW_PRIORITY)
self.lastInstructionTime = time.time()
self.rightAfterBearingCheck = False
self.prevBearing = self.bearingFaced # Why?!?!
def destinationReached(self):
message = messages.DESTINATION_REACHED
print message
# self.voiceOutput.say(message,3)
self.voiceOutput.addToQueue(message, constants.HIGHEST_PRIORITY)
##########################################
# Helper Functions
##########################################
def receiveDataFromArduino(self):
dataReceived = self.serial.serialRead()
dataSplited = dataReceived.split(' ')
dataFiltered = []
for i in range(len(dataSplited)):
if dataSplited[i] != "":
dataFiltered.append(dataSplited[i])
self.headSensor = float(dataFiltered[0])
self.stairSensor = float(dataFiltered[1])
self.bearingFaced = (float(dataFiltered[2])-constants.YPR_OFFSET + constants.COMPASS_OFFSET) % 360
self.stepDetected = float(dataFiltered[3])
def updateIMUCoor(self, currCoor, north, bearingToFace):
if (self.stepDetected > self.prevStep and
abs(self.bearingFaced - self.prevBearing) < constants.WALKING_DEGREE_ERROR and
self.isStairsDetected == False):
print "current bearing to faced is " + str(bearingToFace),
print "current north is " + str(north)
print "x increment is " + str(int(constants.STEP_LENGTH * 100 * math.sin((bearingToFace - north) /180.0 * math.pi)))
imu_new_x = int ((currCoor[0] + constants.STEP_LENGTH * (self.stepDetected - self.prevStep) * 100 *
math.sin((bearingToFace - north) /180.0 * math.pi)))
imu_new_y = int ((currCoor[1] + constants.STEP_LENGTH * (self.stepDetected - self.prevStep) * 100 *
math.cos((bearingToFace - north) / 180.0 * math.pi)))
self.prevStep = self.stepDetected
return [imu_new_x, imu_new_y]
else:
if (self.stepDetected == 1 and abs(self.bearingFaced - self.prevBearing) >= constants.WALKING_DEGREE_ERROR):
print "steps detected but not taken due to turn being made"
if (self.stepDetected == 1 and self.isStairsDetected == True):
print "steps detected but not taken due to stairs detected."
return currCoor
def estimateCurrentPosition(self, imuCoor, wifiCoor, north):
currCoor = []
timeElapsed = time.time() - self.lastUpdatedTime
approx_x_travelled = (timeElapsed * constants.USER_SPEED *
math.sin((self.bearingFaced - north) / 180.0 * math.pi))
approx_y_travelled = (timeElapsed * constants.USER_SPEED *
math.cos((self.bearingFaced - north) / 180.0 * math.pi))
if (approx_x_travelled+currCoor[0] <= wifiCoor[0] ):
if (approx_y_travelled+currCoor[1] <= wifiCoor[1]):
currCoor[0] = (imuCoor[0] + wifiCoor[0])/2
currCoor[1] = (imuCoor[1] + wifiCoor[1])/2
else:
currCoor = imuCoor
self.lastUpdatedTime = time.time()
return currCoor
def warnHeadObstacle(self):
if self.headSensor != 0:
message = messages.HEAD_OBSTACLE_TEMPLATE.format(distance = self.headSensor)
print message
# self.voiceOutput.say(message)
self.voiceOutput.addToQueue(message, constants.HIGH_PRIORITY)
def warnStairs(self):
if self.stairSensor - constants.IR_STAIRS_CONSTANT > constants.STAIR_LIMIT: #downstairs
if self.isDownStairs is False and self.warningStairsCount >= 5:
self.isStairsDetected = True
self.isDownStairs = True
self.voiceOutput.addToQueue(messages.DOWN_STAIRS, constants.HIGH_PRIORITY)
else:
self.warningStairsCount += 1
# self.voiceOutput.say(messages.DOWN_STAIRS)
self.voiceOutput.addToQueue(messages.DOWN_STAIRS, constants.HIGH_PRIORITY)
elif self.stairSensor - constants.IR_STAIRS_CONSTANT < -constants.STAIR_LIMIT: #upstairs
if self.isUpStairs is False and self.warningStairsCount >= 5:
self.isStairsDetected = True
self.isUpStairs = True
self.voiceOutput.addToQueue(messages.UP_STAIRS, constants.HIGH_PRIORITY)
else:
self.warningStairsCount += 1
# self.voiceOutput.say(messages.UP_STAIRS)
else:
self.isDownStairs = False
self.isUpStairs = False
self.isStairsDetected = False
self.warningStairsCount = 0
self.onPlatform = True
# print self.stairSensor,
# print self.prevStairSensor,
# print " ",
# print constants.STAIR_LIMIT
# if self.isDownStairs:
# self.isStairsDetected ^= True
# self.isDownStairs ^= True
# self.onPlatform = True #set to check if user is on platform
# self.warningMessage = ""
# elif self.isUpStairs is False:
# self.isStairsDetected ^= True
# self.isUpStairs ^= True
# self.warningMessage = messages.UP_STAIRS
# self.voiceOutput.say(messages.UP_STAIRS)
# #self.prevStairSensor = self.stairSensor
def guideAlongStairs(self):
if self.stepDetected == 0:
if (self.isStairsDetected is False) and not (self.isUpStairs and self.isDownStairs):
#not on stairs
return
elif (self.isStairsDetected is True) and self.isUpStairs:
message = messages.TAKE_ONE_STEP_TEMPLATE.format(direction = "up")
print " take one step up carefully"
# self.voiceOutput.say(message)
self.voiceOutput.addToQueue(message, constants.HIGH_PRIORITY)
elif (self.isStairsDetected is True) and self.isDownStairs:
message = messages.TAKE_ONE_STEP_TEMPLATE.format(direction = "down")
print " take one step down carefully"
# self.voiceOutput.say(message)
self.voiceOutput.addToQueue(message, constants.HIGH_PRIORITY)
return
else:
#user taking a step
if self.stepsOnPlatform:
self.stepsOnPlatform += 1
if self.stepsOnPlatform >= constants.MAX_ON_PLATFORM_STEPS: #user is not on platform
self.onPlatform = False
self.stepsOnPlatform = 0
return
