diff --git a/main_loop.py b/main_loop.py index 5c4c524..48c1038 100644 --- a/main_loop.py +++ b/main_loop.py @@ -280,6 +280,7 @@ def main(connection=None): const.FUNCTION_UPDATE_PID: robot_control.update_pid_values, const.FUNCTION_REQUEST_CONFIG: robot_control.send_config, const.FUNCTION_REQUEST_PID: robot_control.send_pid_factors, + const.FUNCTION_SET_COIL_INDEX: robot_control.on_set_coil_index, } get_function[const.PUB_MESSAGES.index(topic[i])](buf[i]["data"]) diff --git a/robot/constants.py b/robot/constants.py index bc6d3fd..5938706 100644 --- a/robot/constants.py +++ b/robot/constants.py @@ -161,6 +161,7 @@ "Neuronavigation to Robot: Update robot control pid factors", "Neuronavigation to Robot: Request config", "Dashboard to Robot: Request pid factors", + "Neuronavigation to Robot: Set coil index" ] FUNCTION_CONNECT_TO_ROBOT = 0 @@ -182,3 +183,4 @@ FUNCTION_UPDATE_PID = 16 FUNCTION_REQUEST_CONFIG = 17 FUNCTION_REQUEST_PID = 18 +FUNCTION_SET_COIL_INDEX = 19 diff --git a/robot/control/robot_control.py b/robot/control/robot_control.py index 6b1f7af..174e729 100644 --- a/robot/control/robot_control.py +++ b/robot/control/robot_control.py @@ -63,6 +63,8 @@ def __init__(self, remote_control, config, site_config, robot_config, connection listener = keyboard.Listener(on_press=self.on_keypress) listener.start() + self.coil_index = 2 # Default coil index from tracker system + self.target_set = False self.m_target_to_head = None self.target_reached = False @@ -240,9 +242,12 @@ def on_update_tracker_poses(self, data): poses = data["poses"] visibilities = data["visibilities"] self.tracker.SetCoordinates( - np.vstack([poses[0], poses[1], poses[2]]), visibilities + np.vstack([poses[0], poses[1], poses[self.coil_index]]), [visibilities[0], visibilities[1], visibilities[self.coil_index]] ) + def on_set_coil_index(self, data): + self.coil_index = data["coil_idx"] + def on_create_point(self, data): if self.create_calibration_point(): if self.connection: