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)
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