コード例 #1
0
ファイル: Move.py プロジェクト: HugoCMU/SolarTree
    def get_move_vector(self):
        '''
            Performs movement based on gradient direction of sensor readings.
            Returns vector direction of movement
        '''

        # Read and Smooth raw data from sensors
        raw_data = (sample() for i in xrange(params.p['DATA_SAMPLE_SIZE']))
        self.smooth_data = smoothData(raw_data)

        # Combine readings together using sensor weights
        for data, sensr in zip(self.smooth_data, sensors.s.iterkeys()):

            # Determine position vector for sensor data (with respect to robot frame)
            pos_vector = sensors.to_robot(sensr, data)

            # Second element in sensor dictionary entry is sensor weight
            sensor_weight = sensors.s[sensr][1]

            # Multiply pos readings by sensor weight
            weighted_vector = np.multiply(sensor_weight, pos_vector).tolist()

            self.weighted_pos_vectors.append(weighted_vector)

            # Debug print statements
            # print("sensr, ", sensr)
            # print("data, ", data)
            # print("pos_vector, ", pos_vector)
            # print("sensor_weight, ", sensor_weight)
            # print("weighted_vector, ", weighted_vector)

        # Combine weighted position vectors to get ultimate direction vector
        self.direction_vector = np.mean(np.array(self.weighted_pos_vectors), axis=0)
コード例 #2
0
ファイル: navigation.py プロジェクト: HugoCMU/SolarTree
def get_move_vector(move):

    # Read in raw data from sensors
    move.raw_data = readData()

    # Smooth raw data from sensors
    move.smooth_data = smoothData(move.raw_data)

    # Determine position vectors for sensor data (with respect to robot frame)
    move.pos_vectors = [sensors.to_robot(sensors.sensor_names[i], move.smooth_data[i])
                        for i in range(len(sensors.sensor_names))]

    # Determine position vectors for sensor data (with respect to global frame)
    move.global_pos_vectors = [sensors.to_global(sensors.sensor_names[i], move.smooth_data[i], move.initial_pos)
                        for i in range(len(sensors.sensor_names))]

    # Combine readings together using sensor weights
    for i in range(len(move.pos_vectors)):
        # Second element in sensor dictionary entry is sensor weight
        sensor_weight = sensors.s[sensors.sensor_names[i]][1]
        # Multiply pos readings by sensor weight
        weighted_vector = np.multiply(
            sensor_weight, move.pos_vectors[i]).tolist()
        move.weighted_pos_vectors.append(
            [item for sublist in weighted_vector for item in sublist])  # Flatten result

    # Combine weighted position vectors to get ultimate direction vector
    move.direction_vector = np.mean(
        np.array(move.weighted_pos_vectors), axis=0)

    # Return move object
    return move