def test_straight_settings(self, x, direction, output_stream):
mot_vals = get_motor_vals_for_dir(x, direction)
try:
return self.straight_settings_history[direction][str(mot_vals)]
except KeyError:
'''We haven't done that one before.'''
set_motors(direction, *mot_vals)
try:
self.spin_for_safe_straight(direction)
(pos_list, orient_list,
num_good_points) = self.get_position_data_batch(
direction, self.data_points_for_straighten)
except MyException:
return
if num_good_points < 0.8 * self.data_points_for_straighten:
print("Not enough good data.")
return self.test_straight_settings(mot_vals, direction,
output_stream)
(center, radius, residual, sign) = maths.lsq(pos_list, orient_list)
distance_traveled = np.linalg.norm(pos_list[-1] - pos_list[0])
travel_dir = maths.get_travel_dir(pos_list[0], pos_list[-1],
orient_list[0],
self.expected_travel_dirs[direction])
self.straight_settings_history[direction][str(mot_vals)] = (
radius,
travel_dir,
distance_traveled,
)
#print("settings_history%s = %f"%(str(motor_values), radius))
#print("settings_history: %s"%(str(self.settings_history)))
output_stream.write("%d, %d, %d, %f, %f, %f\n" %
(mot_vals[0], mot_vals[1], mot_vals[2], radius,
sign, distance_traveled))
output_stream.flush()
return self.test_straight_settings(mot_vals, direction, output_stream)
def test_spin_settings(self, motor_values, cw_q, output_stream):
try:
val = self.spin_settings_history[cw_q][str(motor_values)]
except KeyError:
'''We haven't done that one before.'''
else:
return val
set_motors(7-cw_q, *motor_values)
try:
(pos_list, orient_list, num_good_points) = self.get_position_data_batch(7-cw_q, self.data_points_for_straighten)
except MyException:
return
if num_good_points<0.8*self.data_points_for_straighten:
print("Not enough good data.")
return self.test_spin_settings(motor_values, cw_q, output_stream)
(center, radius, residual, sign) = maths.lsq(pos_list, orient_list)
radial_velocity = maths.get_radial_velocity(orient_list)
print("mean delta orient: %f"%(radial_velocity))
print("Radius: %f"%(radius))
self.spin_settings_history[cw_q][str(motor_values)] = (radius, radial_velocity)
orient_changed = orient_list[-1]-orient_list[0]
output_stream.write("%f, %f, %f, %f, %f, %f\n"%(motor_values[0], motor_values[1], motor_values[2], radius, radial_velocity, orient_changed))
output_stream.flush()
return self.test_spin_settings(motor_values, cw_q, output_stream)
def test_spin_settings(self, motor_values, cw_q, output_stream):
try:
val = self.spin_settings_history[cw_q][str(motor_values)]
except KeyError:
'''We haven't done that one before.'''
else:
return val
set_motors(7 - cw_q, *motor_values)
try:
(pos_list, orient_list,
num_good_points) = self.get_position_data_batch(
7 - cw_q, self.data_points_for_straighten)
except MyException:
return
if num_good_points < 0.8 * self.data_points_for_straighten:
print("Not enough good data.")
return self.test_spin_settings(motor_values, cw_q, output_stream)
(center, radius, residual, sign) = maths.lsq(pos_list, orient_list)
radial_velocity = maths.get_radial_velocity(orient_list)
print("mean delta orient: %f" % (radial_velocity))
print("Radius: %f" % (radius))
self.spin_settings_history[cw_q][str(motor_values)] = (radius,
radial_velocity)
orient_changed = orient_list[-1] - orient_list[0]
output_stream.write("%f, %f, %f, %f, %f, %f\n" %
(motor_values[0], motor_values[1], motor_values[2],
radius, radial_velocity, orient_changed))
output_stream.flush()
return self.test_spin_settings(motor_values, cw_q, output_stream)
def test_straight_settings(self, x, direction, output_stream):
mot_vals = get_motor_vals_for_dir(x, direction)
try:
return self.straight_settings_history[direction][str(mot_vals)]
except KeyError:
'''We haven't done that one before.'''
set_motors(direction,*mot_vals)
try:
self.spin_for_safe_straight(direction)
(pos_list, orient_list, num_good_points) = self.get_position_data_batch(direction, self.data_points_for_straighten)
except MyException:
return
if num_good_points<0.8*self.data_points_for_straighten:
print("Not enough good data.")
return self.test_straight_settings(mot_vals, direction, output_stream)
(center, radius, residual, sign) = maths.lsq(pos_list, orient_list)
distance_traveled = np.linalg.norm(pos_list[-1]-pos_list[0])
travel_dir = maths.get_travel_dir(pos_list[0],pos_list[-1],orient_list[0],self.expected_travel_dirs[direction])
self.straight_settings_history[direction][str(mot_vals)] = (radius, travel_dir, distance_traveled,)
#print("settings_history%s = %f"%(str(motor_values), radius))
#print("settings_history: %s"%(str(self.settings_history)))
output_stream.write("%d, %d, %d, %f, %f, %f\n"%(mot_vals[0], mot_vals[1], mot_vals[2], radius, sign, distance_traveled))
output_stream.flush()
return self.test_straight_settings(mot_vals, direction, output_stream)
