Exemple #1
0
    def __init__(self):

        self.bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards

        self.mode = START_JOURNEY
        self.coordX = 0
        self.coordY = 0

        # invoking hmc5833l i2c bus inside mpu6050
        self.bus.write_byte_data(mpu_address, 0x6A, 0)
        self.bus.write_byte_data(mpu_address, 0x37, 2)
        self.bus.write_byte_data(mpu_address, power_mgmt_1, 0)

        self.bus.write_byte_data(hmc_address, 0, 0b01110000) # Set to 8 samples @ 15Hz
        self.bus.write_byte_data(hmc_address, 1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
        self.bus.write_byte_data(hmc_address, 2, 0b00000000) # Continuous sampling


        # update time
        self.go_forward_time = 0
        self.turn_time = 0
        self.stairs_time = 0

        # initializing variables needed for imu readings

        self.accel_val = Vector(0, 0, 0)

        # used for step detection (peak-to-peak detection)
        self.peak_direction = MINIMA
        self.accel_maxima = Vector(0, 0, 0)
        self.accel_minima = Vector(0, 0, 0)
        self.num_steps = 0
        self.sample_new = Vector(0, 0, 0)
        self.time_window = 0
        self.peak_threshold = 0
        self.steps = 0

        # used for calculating the distance (stride-length and total distance within update time)
        self.accel_list = []
        self.distance = 0
        self.total_distance = 0

        # used for accelerometer filtering
        self.gravity = Vector(0, 0, 0)
        self.moving_index = 0
        self.accel_filter_list = []

        self.calculate_distance = False
        self.first_time = True
        self.startNavi = True

        # used for compass heading
        self.compass_val = Vector(0, 0, 0)
        self.heading = 0

        # used for barometer
        self.barometer = BMP085(BMP085_STANDARD, BMP085_I2CADDR, 1)
        self.altitude = 0

        # download map
        self.mapinfolist = MapInfoList()
Exemple #2
0
class Navigation:

    def __init__(self):

        self.bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards

        self.mode = START_JOURNEY
        self.coordX = 0
        self.coordY = 0

        # invoking hmc5833l i2c bus inside mpu6050
        self.bus.write_byte_data(mpu_address, 0x6A, 0)
        self.bus.write_byte_data(mpu_address, 0x37, 2)
        self.bus.write_byte_data(mpu_address, power_mgmt_1, 0)

        self.bus.write_byte_data(hmc_address, 0, 0b01110000) # Set to 8 samples @ 15Hz
        self.bus.write_byte_data(hmc_address, 1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
        self.bus.write_byte_data(hmc_address, 2, 0b00000000) # Continuous sampling


        # update time
        self.go_forward_time = 0
        self.turn_time = 0
        self.stairs_time = 0

        # initializing variables needed for imu readings

        self.accel_val = Vector(0, 0, 0)

        # used for step detection (peak-to-peak detection)
        self.peak_direction = MINIMA
        self.accel_maxima = Vector(0, 0, 0)
        self.accel_minima = Vector(0, 0, 0)
        self.num_steps = 0
        self.sample_new = Vector(0, 0, 0)
        self.time_window = 0
        self.peak_threshold = 0
        self.steps = 0

        # used for calculating the distance (stride-length and total distance within update time)
        self.accel_list = []
        self.distance = 0
        self.total_distance = 0

        # used for accelerometer filtering
        self.gravity = Vector(0, 0, 0)
        self.moving_index = 0
        self.accel_filter_list = []

        self.calculate_distance = False
        self.first_time = True
        self.startNavi = True

        # used for compass heading
        self.compass_val = Vector(0, 0, 0)
        self.heading = 0

        # used for barometer
        self.barometer = BMP085(BMP085_STANDARD, BMP085_I2CADDR, 1)
        self.altitude = 0

        # download map
        self.mapinfolist = MapInfoList()

    # call this method when receive start and destination id
    def getShortestPath(self, startBuilding, startLevel, startNode, endBuilding, endLevel, endNode):
        tup = self.mapinfolist.shortestPath(startBuilding, startLevel, startNode, endBuilding, endLevel, endNode)

    def execute(self, queue):
        
        ##### initialization stage #####

        accel_xout = read_word_2c(self.bus, mpu_address, 0x3b)
        accel_yout = read_word_2c(self.bus, mpu_address, 0x3d)
        accel_zout = read_word_2c(self.bus, mpu_address, 0x3f)

        for i in range(4) :
            accel_xout = read_word_2c(self.bus, mpu_address, 0x3b)
            accel_yout = read_word_2c(self.bus, mpu_address, 0x3d) 
            accel_zout = read_word_2c(self.bus, mpu_address, 0x3f)

            self.accel_val = Vector(accel_xout, accel_yout, accel_zout)
            self.accel_filter_list.append(self.accel_val)


        print "\n\nSTRIDE_COEFFICIENT: ", STRIDE_COEFFICIENT
        print "PEAK THRESHOLD", PEAK_THRESHOLD
        print "TIME THRESHOLD", TIME_THRESHOLD
        print "START!!"

        while(True):

            ### mode GO_FORWARD will update the distance and heading ###

            ##### get accelermeter reading #####

            # filter accelerometer values
            accel_xout = read_word_2c(self.bus, mpu_address, 0x3b)
            accel_yout = read_word_2c(self.bus, mpu_address, 0x3d)
            accel_zout = read_word_2c(self.bus, mpu_address, 0x3f)

            self.accel_val = Vector(accel_xout, accel_yout, accel_zout)

            self.gravity.x = HIGH_PASS * self.gravity.x + (1 - HIGH_PASS) * self.accel_val.x
            self.gravity.y = 0
            self.gravity.z = HIGH_PASS * self.gravity.z + (1 - HIGH_PASS) * self.accel_val.z

            self.accel_val.x = self.accel_val.x - self.gravity.x
            self.accel_val.y = self.accel_val.y - self.gravity.y
            self.accel_val.z = self.accel_val.z - self.gravity.z
            
            self.accel_val.x = (self.accel_filter_list[0].x + self.accel_filter_list[1].x + self.accel_filter_list[2].x + self.accel_filter_list[3].x + self.accel_val.x) / 5
            self.accel_val.y = (self.accel_filter_list[0].y + self.accel_filter_list[1].y + self.accel_filter_list[2].y + self.accel_filter_list[3].y + self.accel_val.y) / 5
            self.accel_val.z = (self.accel_filter_list[0].z + self.accel_filter_list[1].z + self.accel_filter_list[2].z + self.accel_filter_list[3].z + self.accel_val.z) / 5

            self.accel_filter_list.insert(self.moving_index, self.accel_val)

            self.moving_index = (self.moving_index + 1) % 4

            # step detection - peak-to-peak detection
            if(not self.first_time):
                if( math.fabs( self.sample_new.y - self.accel_val.y ) >= ACCEL_THRESHOLD):
                    self.sample_new = self.accel_val
                    self.accel_list.append(self.accel_val)

                    # looking for a minima peak
                    if(self.peak_direction == MINIMA):

                        if(self.accel_val.y  > self.accel_maxima.y):
                            self.accel_maxima = self.accel_val
                        
                        else:

                            if(self.calculate_distance):
                                #if(self.mode == GO_FORWARD):
                                stride = getStrideLength(self.accel_list)
                                print "--------------- STRIDE", stride
                                self.distance += stride
                                self.total_distance += stride
                                self.accel_list = []
                                self.calculate_distance = False

                            if( self.accel_maxima.y - self.accel_val.y >= self.peak_threshold ):
                                if(time.time() - self.time_window >= TIME_THRESHOLD):
                                    # a maxima has been detected and a step is detected
                                    #if(self.mode == GO_FORWARD):
                                        # #beep at every step
                                        # q_step.put(1)

                                    self.num_steps += 1
                                    self.steps += 1

                                    self.peak_direction = MAXIMA
                                    self.accel_minima = self.accel_val
                                    self.time_window = time.time()
                                    print "\n--------------- PEAK DETECTED MINIMA", self.num_steps
                                    self.peak_threshold = PEAK_THRESHOLD
                                    self.calculate_distance = True


                    # looking for a maxima peak
                    if( self.peak_direction == MAXIMA ):
                        
                        if( self.accel_val.y < self.accel_minima.y):
                            self.accel_minima = self.accel_val

                        else:

                            if(self.calculate_distance):
                                #if(self.mode == GO_FORWARD):
                                stride = getStrideLength(self.accel_list)
                                print "--------------- STRIDE", stride
                                self.distance += stride
                                self.total_distance += stride
                                self.calculate_distance = False
                                self.accel_list = []


                            if(self.accel_val.y - self.accel_minima.y >= self.peak_threshold ):
                                if(time.time() - self.time_window >= TIME_THRESHOLD):
                                    # a maxima has been detected and a step is detected
                                    #if(self.mode == GO_FORWARD):
                                        # #beep at every step
                                        # q_step.put(1)
                                        
                                    self.num_steps += 1
                                    self.steps += 1

                                    self.peak_direction = MINIMA
                                    self.accel_maxima = self.accel_val
                                    self.time_window = time.time()
                                    print "\n--------------- PEAK DETECTED MAXIMA", self.num_steps
                                    self.peak_threshold = PEAK_THRESHOLD
                                    self.calculate_distance = True

            else:
                self.peak_direction = MINIMA
                self.peak_threshold = PEAK_THRESHOLD / 2
                self.accel_maxima = self.accel_val
                self.first_time = False
                self.sample_new = self.accel_val
                self.time_window = time.time()   

            # reading compass values
            compass_xout = read_word_2c(self.bus, hmc_address, 3) - COMPASS_X_AXIS
            compass_yout = read_word_2c(self.bus, hmc_address, 7) 
            compass_zout = read_word_2c(self.bus, hmc_address, 5) - COMPASS_Z_AXIS

            self.compass_val = Vector(compass_xout, compass_yout, compass_zout)
            self.heading = GetHeading(self.compass_val)

            ### mode STAIRS will update barometer
            if(self.mode == STAIRS):
                self.altitude = self.barometer.read_altitude(SEA_LEVEL)

            # # check qrcode updates
            # try:
            #     qrstring = q_qrcode.get(block=False)
            #     if qrstring != None:
            #             #mapid-nodeid eg. 3-02
            #             print qrstring
            #             ids = qrstring.split('-')
            #             mapid = int(ids[0])
            #             nodeid = int(ids[1])
            #             dic = self.mapinfolist.updateCurrentCoordinates(mapid, nodeid)
            #             self.coordX = dic[COORDX]
            #             self.coordY = dic[COORDY]
            # # Queue.empty
            # except Exception:
            #     #ignore
            #     pass

            ##### check state machine #####
            result = self.mapinfolist.giveDirection(self.distance, self.heading, self.altitude, self.coordX, self.coordY, self.steps)
            self.steps = 0
            self.distance = 0
            self.mode = result[MODE]
            self.coordX = result[COORDX]
            self.coordY = result[COORDY] 
            feedback = ""

            if(self.mode == START_JOURNEY):
                numberBuildings = result[NUMBER_OF_BUILDINGS]
                
                #feedback is "You have to walk through " + str(numberBuildings) + " building(s)"
                # feedback = "sj," + str(numberBuildings)
                print "\n\nMODE: START_JOURNEY ---\n" + "sj," + str(numberBuildings)

            if(self.mode == START_BUILDING):
                # numberNodes = result[NUMBER_NODES]
                currentBuilding = result[CURRENT_BUILDING]
                currentNode = result[CURRENT_NODE]
                # building = "COM 1"
                # level = 2

                # if(currentBuilding == 1):
                    # building = "COM 2"
                    # level = 2
                # elif(currentBuilding == 2):
                    # building = "COM 2"
                    # level = 3

                # feedback is "You are currently at building " + str(building) + " level " + str(level) + "\nYou have to walk pass " + str(numberNodes) + " nodes" "\nNow starting at node " + str(currentNode)                  
                # feedback = "sb," + str(building) + "," + str(level) + "," + str(numberNodes) + "," + str(currentNode)
                feedback = "bn," + str(currentBuilding) + "," + str(currentNode)
                self.startNavi = True
                print "\n\n--- MODE: START_BUILDING ---\n" + feedback + "\nCOORDX: " + str(self.coordX) + "   COORDY: " + str(self.coordY)

            elif(self.mode == REACH_NODE):
                currentNode = result[CURRENT_NODE]
                # feedback is "You have reached node " + str(currentNode)
                feedback = "rn," + str(currentNode)
                print "\n\n--- MODE: REACH_NODE ---\n" + feedback + "\nCOORDX: " + str(self.coordX) + "   COORDY: " + str(self.coordY)

            elif (self.mode == TURN):
                if(self.startNavi == False):
                    if(time.time() - self.turn_time >= TURN_UPDATE_TIME):
                        isLeft = result[LEFTORRIGHT]
                        angle = int(result[ANGLE])
                        self.turn_time = time.time()
                        if(isLeft == LEFT):

                            # feedback is "Turn left by " + str(angle) + " degrees"
                            feedback = "tl"
                        else:
                            
                            # feedback is "Turn right by " + str(angle) + " degrees"
                            feedback = "tr"
                        print "\n\n--- MODE: TURN ---\n" + feedback + " " + str(angle)
                else:
                    self.startNavi = False
                    self.turn_time = time.time()

            elif(self.mode == GO_FORWARD):
                if(time.time() - self.go_forward_time >= GO_FORWARD_UPDATE_TIME):
                    self.go_forward_time = time.time()
                    feedback = "gf"
                    print "\n\n--- MODE: GO_FORWARD ---\n" + "Go forward" + "\nCOORDX: " + str(self.coordX) + "   COORDY: " + str(self.coordY)

            elif(self.mode == STAIRS):
                if(time.time() - self.stairs_time >= STAIRS_UPDATE_TIME):
                    self.stairs_time = time.time()
                    feedback = "gu"
                    print "\n\n--- MODE: STAIRS ---\n" + "Stairs"

            elif(self.mode == ARRIVE_DESTINATION):
                feedback = "r"
                print "\n\nMODE: ARRIVE DESTINATION ---"
                queue.put(feedback)
                break

            if feedback != "":
                queue.put(feedback)