Refactor & add _serialize_data

This commit is contained in:
Simon Alibert
2025-04-11 11:01:12 +02:00
parent 27cb0c40bd
commit d32daebf75
7 changed files with 78 additions and 104 deletions

View File

@@ -237,7 +237,7 @@ class MockInstructionPacket(MockDynamixelPacketv2):
+2 is for the length bytes,
+2 is for the CRC at the end.
"""
data = DynamixelMotorsBus._split_int_to_bytes(value, data_length)
data = DynamixelMotorsBus._split_into_byte_chunks(value, data_length)
params = [
dxl.DXL_LOBYTE(start_address),
dxl.DXL_HIBYTE(start_address),
@@ -315,7 +315,7 @@ class MockInstructionPacket(MockDynamixelPacketv2):
"""
data = []
for id_, value in ids_values.items():
split_value = DynamixelMotorsBus._split_int_to_bytes(value, data_length)
split_value = DynamixelMotorsBus._split_into_byte_chunks(value, data_length)
data += [id_, *split_value]
params = [
dxl.DXL_LOBYTE(start_address),
@@ -389,7 +389,7 @@ class MockStatusPacket(MockDynamixelPacketv2):
Returns:
bytes: The raw 'Present_Position' status packet ready to be sent through serial.
"""
params = DynamixelMotorsBus._split_int_to_bytes(value, param_length)
params = DynamixelMotorsBus._split_into_byte_chunks(value, param_length)
length = param_length + 4
return cls.build(dxl_id, params=params, length=length)

View File

@@ -49,7 +49,7 @@ class MockFeetechPacket(abc.ABC):
for id_ in range(2, len(packet) - 1): # except header & checksum
checksum += packet[id_]
packet[-1] = scs.SCS_LOBYTE(~checksum)
packet[-1] = ~checksum & 0xFF
return packet
@@ -139,7 +139,7 @@ class MockInstructionPacket(MockFeetechPacket):
+1 is for the length bytes,
+1 is for the checksum at the end.
"""
data = FeetechMotorsBus._split_int_to_bytes(value, data_length)
data = FeetechMotorsBus._split_into_byte_chunks(value, data_length)
params = [start_address, *data]
length = data_length + 3
return cls.build(scs_id=scs_id, params=params, length=length, instruct_type="Write")
@@ -201,7 +201,7 @@ class MockInstructionPacket(MockFeetechPacket):
"""
data = []
for id_, value in ids_values.items():
split_value = FeetechMotorsBus._split_int_to_bytes(value, data_length)
split_value = FeetechMotorsBus._split_into_byte_chunks(value, data_length)
data += [id_, *split_value]
params = [start_address, data_length, *data]
length = len(ids_values) * (1 + data_length) + 4
@@ -258,7 +258,7 @@ class MockStatusPacket(MockFeetechPacket):
Returns:
bytes: The raw 'Sync Read' status packet ready to be sent through serial.
"""
params = FeetechMotorsBus._split_int_to_bytes(value, param_length)
params = FeetechMotorsBus._split_into_byte_chunks(value, param_length)
length = param_length + 2
return cls.build(scs_id, params=params, length=length)

View File

@@ -62,7 +62,7 @@ def test_autouse_patch():
@pytest.mark.parametrize(
"value, n_bytes, expected",
"value, length, expected",
[
(0x12, 1, [0x12]),
(0x1234, 2, [0x34, 0x12]),
@@ -86,24 +86,24 @@ def test_autouse_patch():
"max four bytes",
],
) # fmt: skip
def test_split_int_to_bytes(value, n_bytes, expected):
assert DynamixelMotorsBus._split_int_to_bytes(value, n_bytes) == expected
def test_serialize_data(value, length, expected):
assert DynamixelMotorsBus._serialize_data(value, length) == expected
def test_split_int_to_bytes_invalid_n_bytes():
def test_serialize_data_invalid_length():
with pytest.raises(NotImplementedError):
DynamixelMotorsBus._split_int_to_bytes(100, 3)
DynamixelMotorsBus._serialize_data(100, 3)
def test_split_int_to_bytes_negative_numbers():
def test_serialize_data_negative_numbers():
with pytest.raises(ValueError):
neg = DynamixelMotorsBus._split_int_to_bytes(-1, 1)
neg = DynamixelMotorsBus._serialize_data(-1, 1)
print(neg)
def test_split_int_to_bytes_large_number():
def test_serialize_data_large_number():
with pytest.raises(ValueError):
DynamixelMotorsBus._split_int_to_bytes(2**32, 4) # 4-byte max is 0xFFFFFFFF
DynamixelMotorsBus._serialize_data(2**32, 4) # 4-byte max is 0xFFFFFFFF
def test_abc_implementation(dummy_motors):

View File

@@ -61,7 +61,7 @@ def test_autouse_patch():
@pytest.mark.parametrize(
"value, n_bytes, expected",
"value, length, expected",
[
(0x12, 1, [0x12]),
(0x1234, 2, [0x34, 0x12]),
@@ -85,24 +85,24 @@ def test_autouse_patch():
"max four bytes",
],
) # fmt: skip
def test_split_int_to_bytes(value, n_bytes, expected):
assert FeetechMotorsBus._split_int_to_bytes(value, n_bytes) == expected
def test_serialize_data(value, length, expected):
assert FeetechMotorsBus._serialize_data(value, length) == expected
def test_split_int_to_bytes_invalid_n_bytes():
def test_serialize_data_invalid_length():
with pytest.raises(NotImplementedError):
FeetechMotorsBus._split_int_to_bytes(100, 3)
FeetechMotorsBus._serialize_data(100, 3)
def test_split_int_to_bytes_negative_numbers():
def test_serialize_data_negative_numbers():
with pytest.raises(ValueError):
neg = FeetechMotorsBus._split_int_to_bytes(-1, 1)
neg = FeetechMotorsBus._serialize_data(-1, 1)
print(neg)
def test_split_int_to_bytes_large_number():
def test_serialize_data_large_number():
with pytest.raises(ValueError):
FeetechMotorsBus._split_int_to_bytes(2**32, 4) # 4-byte max is 0xFFFFFFFF
FeetechMotorsBus._serialize_data(2**32, 4) # 4-byte max is 0xFFFFFFFF
def test_abc_implementation(dummy_motors):