fix(dynamixel): add Protocol 1 motor discovery via sequential ping

Broadcast Ping is unavailable on Protocol 1.0, so _find_single_motor now
branches: Protocol 2 keeps the broadcast-based scan while Protocol 1
scans IDs sequentially with individual pings, mirroring Feetech.
This commit is contained in:
CarolinePascal
2026-07-05 17:13:20 +02:00
parent 5a1527d868
commit fc5231b5d0
+28
View File
@@ -150,6 +150,11 @@ class DynamixelMotorsBus(SerialMotorsBus):
self._assert_motors_exist()
def _find_single_motor(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
if self.protocol_version == 1:
return self._find_single_motor_p1(motor, initial_baudrate)
return self._find_single_motor_p2(motor, initial_baudrate)
def _find_single_motor_p2(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
model = self.motors[motor].model
search_baudrates = (
[initial_baudrate] if initial_baudrate is not None else self.model_baudrate_table[model]
@@ -171,6 +176,29 @@ class DynamixelMotorsBus(SerialMotorsBus):
raise RuntimeError(f"Motor '{motor}' (model '{model}') was not found. Make sure it is connected.")
def _find_single_motor_p1(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
# Protocol 1.0 has no Broadcast Ping, so scan IDs sequentially with individual pings.
model = self.motors[motor].model
search_baudrates = (
[initial_baudrate] if initial_baudrate is not None else self.model_baudrate_table[model]
)
expected_model_nb = self.model_number_table[model]
for baudrate in search_baudrates:
self.set_baudrate(baudrate)
for id_ in range(dxl.MAX_ID + 1):
found_model = self.ping(id_)
if found_model is not None:
if found_model != expected_model_nb:
raise RuntimeError(
f"Found one motor on {baudrate=} with id={id_} but it has a "
f"model number '{found_model}' different than the one expected: '{expected_model_nb}'. "
f"Make sure you are connected only connected to the '{motor}' motor (model '{model}')."
)
return baudrate, id_
raise RuntimeError(f"Motor '{motor}' (model '{model}') was not found. Make sure it is connected.")
def configure_motors(self, return_delay_time=0) -> None:
# By default, Dynamixel motors have a 500µs delay response time (corresponding to a value of 250 on
# the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0).