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)
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 calculate_desired_directions(self):
forces_dict = defaultdict(lambda: np.array([0.0, 0.0]))
for botId in self.ordered_ids:
if (len(self.pose_dat[botId]) <= 0):
continue
botPos = np.array(
[self.pose_dat[botId][-1][0], self.pose_dat[botId][-1][1]])
# print('{0} @ {1}'.format(botId, botPos))
bl_diff = (botPos / self.droplet_pixel_radius) + np.array(
[1.0, 1.0])
tr_diff = ((np.array([1000.0, 1000.0]) - botPos) /
self.droplet_pixel_radius) + np.array([1.0, 1.0])
bl_wall_force = 1.8 / (bl_diff**1.0)
tr_wall_force = 1.8 / (tr_diff**1.0)
wall_forces = bl_wall_force - tr_wall_force
forces_dict[botId] += wall_forces
# print('\tWALLS: {0}'.format(wall_forces))
for otherBotId in reversed(self.ordered_ids):
if botId is otherBotId:
break
if len(self.pose_dat[otherBotId]) <= 0:
continue
otherBotPos = np.array([
self.pose_dat[otherBotId][-1][0],
self.pose_dat[otherBotId][-1][1]
])
diffVec = (botPos - otherBotPos) / self.droplet_pixel_radius
r = np.linalg.norm(diffVec)
forceVec = ((1.5 * diffVec) / (r**3))
# print('\t({0}, {1}): {2} \t{3} \t{4}'.format(botId, otherBotId, diffVec, r, forceVec))
forces_dict[botId] += forceVec
forces_dict[otherBotId] -= forceVec
direction_dict = defaultdict()
for botId in self.ordered_ids:
delta_theta = 0.0
(r, theta) = (hypot(*forces_dict[botId]),
degrees(np.arctan2(*(forces_dict[botId][::-1]))))
if r < 0.1:
direction_dict[botId] = self.get_rand_dir()
elif len(self.pose_dat[botId]) <= 0:
direction_dict[botId] = self.get_rand_dir()
else:
delta_theta = CustomMaths.pretty_angle(
(theta - 90) - self.pose_dat[botId][-1][2])
if abs(delta_theta) <= 50:
direction_dict[botId] = 1 #straight forward
elif abs(delta_theta) >= 130:
direction_dict[botId] = 2 #straight backward
elif delta_theta > 0:
direction_dict[botId] = 4 #turn left
elif delta_theta < 0:
direction_dict[botId] = 3 #turn right
# print('{0}: ({1}, {2}, {3}): {4}'.format(botId, r, theta, delta_theta, direction_dict[botId]))
return direction_dict
def calculate_desired_directions(self):
forces_dict = defaultdict(lambda: np.array([0.0,0.0]))
for botId in self.ordered_ids:
if(len(self.pose_dat[botId])<=0):
continue
botPos = np.array([self.pose_dat[botId][-1][0],
self.pose_dat[botId][-1][1]])
# print('{0} @ {1}'.format(botId, botPos))
bl_diff = (botPos/self.droplet_pixel_radius)+np.array([1.0,1.0])
tr_diff = ((np.array([1000.0, 1000.0])-botPos)/self.droplet_pixel_radius)+np.array([1.0,1.0])
bl_wall_force = 1.8/(bl_diff**1.0)
tr_wall_force = 1.8/(tr_diff**1.0)
wall_forces = bl_wall_force-tr_wall_force
forces_dict[botId]+=wall_forces
# print('\tWALLS: {0}'.format(wall_forces))
for otherBotId in reversed(self.ordered_ids):
if botId is otherBotId:
break
if len(self.pose_dat[otherBotId])<=0:
continue
otherBotPos = np.array([self.pose_dat[otherBotId][-1][0],
self.pose_dat[otherBotId][-1][1]])
diffVec = (botPos - otherBotPos)/self.droplet_pixel_radius
r = np.linalg.norm(diffVec)
forceVec = ((1.5*diffVec)/(r**3))
# print('\t({0}, {1}): {2} \t{3} \t{4}'.format(botId, otherBotId, diffVec, r, forceVec))
forces_dict[botId] += forceVec
forces_dict[otherBotId] -= forceVec
direction_dict = defaultdict()
for botId in self.ordered_ids:
delta_theta = 0.0
(r, theta) = (hypot(*forces_dict[botId]), degrees(np.arctan2(*(forces_dict[botId][::-1]))))
if r<0.1:
direction_dict[botId] = self.get_rand_dir()
elif len(self.pose_dat[botId])<=0:
direction_dict[botId] = self.get_rand_dir()
else:
delta_theta = CustomMaths.pretty_angle((theta-90)-self.pose_dat[botId][-1][2])
if abs(delta_theta)<=50:
direction_dict[botId] = 1 #straight forward
elif abs(delta_theta)>=130:
direction_dict[botId] = 2 #straight backward
elif delta_theta>0:
direction_dict[botId] = 4 #turn left
elif delta_theta<0:
direction_dict[botId] = 3 #turn right
# print('{0}: ({1}, {2}, {3}): {4}'.format(botId, r, theta, delta_theta, direction_dict[botId]))
return direction_dict
def main_loop(self):
self.lsq_dat = defaultdict(lambda: (0,0,0,0))
with self.pose_dat_lock:
for robot_id in self.pose_dat.keys():
try:
self.lsq_dat[robot_id] = CustomMaths.lsq_circle_fit(np.array(self.pose_dat[robot_id]))
except TypeError:
continue
self.calculate_dist_per_step()
self.send_message()
self.pose_dat = defaultdict(deque)
self.lsq_dat = defaultdict(lambda: (0,0,0,0))
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 main_loop(self):
self.lsq_dat = defaultdict(lambda: (0, 0, 0, 0))
with self.pose_dat_lock:
for robot_id in self.pose_dat.keys():
try:
self.lsq_dat[robot_id] = CustomMaths.lsq_circle_fit(
np.array(self.pose_dat[robot_id]))
except TypeError:
continue
self.calculate_dist_per_step()
self.send_message()
self.pose_dat = defaultdict(deque)
self.lsq_dat = defaultdict(lambda: (0, 0, 0, 0))
def main():
global serial_buffer
global pose_dat
rr_thread = threading.Thread(target=rr_monitor)
rr_lock.acquire()
rr_thread.start()
serial_thread = threading.Thread(target=serial_monitor)
serial_lock.acquire()
serial_thread.start()
with serial_buffer_lock:
serial_buffer = 'cmd reset'
time.sleep(1)
try:
new_direction = 1
with serial_buffer_lock:
serial_buffer = 'msg {0}{1}{2}{3}{4}{5}{6}{7}{8}{9}{10}{11}{12}{13}{14}{15}{16}{17}{18}{19}'.format(*[chr(96+(new_direction<<2)) for _ in ordered_ids])
last_direction = new_direction
while True:
for _ in range(30):
time.sleep(1)
lsq_dat = {}
for robot_id in ordered_ids:
lsq_dat[robot_id] = None
def get_per_robot_command(bot_id):
value = (96+(new_direction<<2))
if (lsq_dat[bot_id] is not None) and last_direction is not 0:
adjust = calculate_adjust_response(lsq_dat[bot_id], last_direction)
value += adjust
if adjust is 1:
history_dict[bot_id][0][last_direction][0] -= 10
history_dict[bot_id][0][last_direction][1] += 10
elif adjust is 2:
history_dict[bot_id][0][last_direction][0] += 10
history_dict[bot_id][0][last_direction][1] -= 10
return chr(value)
with pose_dat_lock:
for robot_id in pose_dat.keys():
try:
lsq_dat[robot_id] = CustomMaths.lsq_circle_fit(np.array(pose_dat[robot_id]))
except TypeError:
lsq_dat[robot_id] = (0,0,0,0)
calculate_dist_per_step(last_direction)
new_direction = get_rand_dir()
with serial_buffer_lock:
serial_buffer = 'msg {0}{1}{2}{3}{4}{5}{6}{7}{8}{9}{10}{11}{12}{13}{14}{15}{16}{17}{18}{19}'.format(*[get_per_robot_command(bot_id) for bot_id in ordered_ids])
last_direction = new_direction
pose_dat = defaultdict(deque)
for bot in ordered_ids:
print(bot,end='')
for dir in [1,2,3,4]:
print('\t',end='')
print(history_dict[bot][1][dir])
except KeyboardInterrupt:
for bot in ordered_ids:
print(bot)
print("\tAdjusts")
for dir in [1,2,3,4]:
print('\t',end='')
print(history_dict[bot][0][dir])
print("\tDists:")
for dir in [1,2,3,4]:
print('\t',end='')
print(history_dict[bot][1][dir])
rr_lock.release()
rr_thread.join()
serial_lock.release()
serial_thread.join()
sys.exit()
def main():
global serial_buffer
global pose_dat
rr_thread = threading.Thread(target=rr_monitor)
rr_lock.acquire()
rr_thread.start()
serial_thread = threading.Thread(target=serial_monitor)
serial_lock.acquire()
serial_thread.start()
with serial_buffer_lock:
serial_buffer = 'cmd reset'
time.sleep(1)
try:
new_direction = 1
with serial_buffer_lock:
serial_buffer = 'msg {0}{1}{2}{3}{4}{5}{6}{7}{8}{9}{10}{11}{12}{13}{14}{15}{16}{17}{18}{19}'.format(
*[chr(96 + (new_direction << 2)) for _ in ordered_ids])
last_direction = new_direction
while True:
for _ in range(30):
time.sleep(1)
lsq_dat = {}
for robot_id in ordered_ids:
lsq_dat[robot_id] = None
def get_per_robot_command(bot_id):
value = (96 + (new_direction << 2))
if (lsq_dat[bot_id] is not None) and last_direction is not 0:
adjust = calculate_adjust_response(lsq_dat[bot_id],
last_direction)
value += adjust
if adjust is 1:
history_dict[bot_id][0][last_direction][0] -= 10
history_dict[bot_id][0][last_direction][1] += 10
elif adjust is 2:
history_dict[bot_id][0][last_direction][0] += 10
history_dict[bot_id][0][last_direction][1] -= 10
return chr(value)
with pose_dat_lock:
for robot_id in pose_dat.keys():
try:
lsq_dat[robot_id] = CustomMaths.lsq_circle_fit(
np.array(pose_dat[robot_id]))
except TypeError:
lsq_dat[robot_id] = (0, 0, 0, 0)
calculate_dist_per_step(last_direction)
new_direction = get_rand_dir()
with serial_buffer_lock:
serial_buffer = 'msg {0}{1}{2}{3}{4}{5}{6}{7}{8}{9}{10}{11}{12}{13}{14}{15}{16}{17}{18}{19}'.format(
*[
get_per_robot_command(bot_id)
for bot_id in ordered_ids
])
last_direction = new_direction
pose_dat = defaultdict(deque)
for bot in ordered_ids:
print(bot, end='')
for dir in [1, 2, 3, 4]:
print('\t', end='')
print(history_dict[bot][1][dir])
except KeyboardInterrupt:
for bot in ordered_ids:
print(bot)
print("\tAdjusts")
for dir in [1, 2, 3, 4]:
print('\t', end='')
print(history_dict[bot][0][dir])
print("\tDists:")
for dir in [1, 2, 3, 4]:
print('\t', end='')
print(history_dict[bot][1][dir])
rr_lock.release()
rr_thread.join()
serial_lock.release()
serial_thread.join()
sys.exit()
