From ee628a9ace02f2557160a95602e1cbd6b2a12853 Mon Sep 17 00:00:00 2001 From: Marcio Campos <82606834+MarcioCamposJr@users.noreply.github.com> Date: Wed, 1 Jul 2026 13:15:31 -0300 Subject: [PATCH 1/4] ADD: Set coil index to get correct coord from tracker system --- main_loop.py | 1 + robot/constants.py | 4 +++- robot/control/robot_control.py | 7 ++++++- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/main_loop.py b/main_loop.py index 5c4c524..a9a3c4d 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.send_pid_factors, } get_function[const.PUB_MESSAGES.index(topic[i])](buf[i]["data"]) diff --git a/robot/constants.py b/robot/constants.py index 3af0cf5..8ec0929 100644 --- a/robot/constants.py +++ b/robot/constants.py @@ -160,6 +160,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 @@ -172,7 +173,7 @@ FUNCTION_ROBOT_MATRIX_ESTIMATION = 7 FUNCTION_SET_ROBOT_TRANSFORMATION_MATRIX = 8 FUNCTION_COIL_AT_TARGET = 9 -FUNCTION_UPDATE_DISPLACEMENT_TO_TARGET = 10 +FUNCTION_UPDATE_DISPLACEMENT_TO_TARGET = 10 #Corrigir FUNCTION_SET_OBJECTIVE = 11 FUNCTION_SET_FREE_DRIVE = 12 FUNCTION_CHECK_CONNECTION = 13 @@ -181,3 +182,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 f7634b1..5f6e2a3 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 @@ -165,9 +167,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 ) + 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: From 64df9b6991bd369ac9f14053c3f67937f4599a7b Mon Sep 17 00:00:00 2001 From: Marcio Campos <82606834+MarcioCamposJr@users.noreply.github.com> Date: Wed, 1 Jul 2026 15:31:51 -0300 Subject: [PATCH 2/4] FIX: Set correct visibilities from coil associated --- robot/control/robot_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot/control/robot_control.py b/robot/control/robot_control.py index 5f6e2a3..1f98eef 100644 --- a/robot/control/robot_control.py +++ b/robot/control/robot_control.py @@ -167,7 +167,7 @@ def on_update_tracker_poses(self, data): poses = data["poses"] visibilities = data["visibilities"] self.tracker.SetCoordinates( - np.vstack([poses[0], poses[1], poses[self.coil_index]]), visibilities + np.vstack([poses[0], poses[1], poses[self.coil_index]]), np.vstack([visibilities[0], visibilities[1], visibilities[self.coil_index]]) ) def on_set_coil_index(self, data): From 5b16f7a4c86313d21eebba486e0aaced3eddf3fc Mon Sep 17 00:00:00 2001 From: Marcio Campos <82606834+MarcioCamposJr@users.noreply.github.com> Date: Wed, 1 Jul 2026 15:55:54 -0300 Subject: [PATCH 3/4] FIX: Set correct function to set coil index --- main_loop.py | 2 +- robot/control/robot_control.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/main_loop.py b/main_loop.py index a9a3c4d..48c1038 100644 --- a/main_loop.py +++ b/main_loop.py @@ -280,7 +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.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/control/robot_control.py b/robot/control/robot_control.py index fead7cd..174e729 100644 --- a/robot/control/robot_control.py +++ b/robot/control/robot_control.py @@ -242,7 +242,7 @@ def on_update_tracker_poses(self, data): poses = data["poses"] visibilities = data["visibilities"] self.tracker.SetCoordinates( - np.vstack([poses[0], poses[1], poses[self.coil_index]]), np.vstack([visibilities[0], visibilities[1], visibilities[self.coil_index]]) + 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): From 2503507315c141877841ba0daf4fa4f994cf39bb Mon Sep 17 00:00:00 2001 From: Marcio Campos <82606834+MarcioCamposJr@users.noreply.github.com> Date: Thu, 2 Jul 2026 14:51:01 -0300 Subject: [PATCH 4/4] Remove mistake comment --- robot/constants.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot/constants.py b/robot/constants.py index 0340376..5938706 100644 --- a/robot/constants.py +++ b/robot/constants.py @@ -174,7 +174,7 @@ FUNCTION_ROBOT_MATRIX_ESTIMATION = 7 FUNCTION_SET_ROBOT_TRANSFORMATION_MATRIX = 8 FUNCTION_COIL_AT_TARGET = 9 -FUNCTION_UPDATE_DISPLACEMENT_TO_TARGET = 10 #Corrigir +FUNCTION_UPDATE_DISPLACEMENT_TO_TARGET = 10 FUNCTION_SET_OBJECTIVE = 11 FUNCTION_SET_FREE_DRIVE = 12 FUNCTION_CHECK_CONNECTION = 13