diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py index 2d7f50314..28a568887 100644 --- a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py +++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py @@ -9,8 +9,8 @@ class ActuatorPacketId(IntEnum): Enumerator representing each controllable actuator. """ - #: The dropper actuator. - DROPPER = 0 + #: The gripper actuator. + GRIPPER = 0 #: The torpedo launcher actuator. TORPEDO_LAUNCHER = 1 #: The ball drop actuator. Only one actuator is used for both balls. @@ -60,3 +60,24 @@ class ActuatorPollResponsePacket( """ values: int + + @property + def gripper_opened(self) -> bool: + """ + Whether the gripper is opened. + """ + return bool(self.values & (0b0001 << ActuatorPacketId.GRIPPER)) + + @property + def torpedo_launcher_opened(self) -> bool: + """ + Whether the torpedo launcher is opened. + """ + return bool(self.values & (0b0001 << ActuatorPacketId.TORPEDO_LAUNCHER)) + + @property + def ball_drop_opened(self) -> bool: + """ + Whether the ball drop is opened. + """ + return bool(self.values & (0b0001 << ActuatorPacketId.BALL_DROP)) diff --git a/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py b/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py index 014bd090c..0e00479fb 100755 --- a/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py +++ b/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py @@ -2,6 +2,12 @@ import unittest import rospy +from sub_actuator_board.packets import ( + ActuatorPacketId, + ActuatorPollRequestPacket, + ActuatorPollResponsePacket, + ActuatorSetPacket, +) from sub_actuator_board.srv import GetValve, GetValveRequest, SetValve, SetValveRequest @@ -35,6 +41,23 @@ def test_correct_response(self): self.assertFalse(self.get_srv(GetValveRequest(2)).opened) self.assertFalse(self.get_srv(GetValveRequest(0)).opened) + def test_packet(self): + """ + Test that the bytes representation of all packets is correct. + """ + # ActuatorPollRequestPacket + packet = ActuatorPollRequestPacket() + self.assertEqual(bytes(packet), b"7\x01\x03\x01\x00\x00\x04\x0f") + # ActuatorPollResponsePacket + packet = ActuatorPollResponsePacket(0b00111) + self.assertEqual(packet.ball_drop_opened, True) + self.assertEqual(packet.gripper_opened, True) + self.assertEqual(packet.torpedo_launcher_opened, True) + self.assertEqual(bytes(packet), b"7\x01\x03\x02\x01\x00\x07\r!") + # ActuatorSetPacket + packet = ActuatorSetPacket(ActuatorPacketId.TORPEDO_LAUNCHER, True) + self.assertEqual(bytes(packet), b"7\x01\x03\x00\x02\x00\x01\x01\x07\x1d") + if __name__ == "__main__": rospy.init_node("simulated_board_test", anonymous=True) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py index 06f625b9b..4bdf47c32 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py @@ -6,8 +6,8 @@ from functools import lru_cache from typing import ClassVar, get_type_hints -SYNC_CHAR_1 = ord("3") -SYNC_CHAR_2 = ord("7") +SYNC_CHAR_1 = 0x37 +SYNC_CHAR_2 = 0x01 _packet_registry: dict[int, dict[int, type[Packet]]] = {} diff --git a/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py index c58a8c32c..9e1b9c3ce 100755 --- a/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py +++ b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py @@ -16,7 +16,7 @@ class TestPacket(Packet, msg_id=0x47, subclass_id=0x44, payload_format="?Hd"): class BasicApplicationPacketTest(unittest.IsolatedAsyncioTestCase): """ - Tests basic application packt functionality. + Tests basic application packet functionality. """ def test_simple_packet(self): @@ -52,6 +52,14 @@ def test_comparisons(self): with self.assertRaises(TypeError): packet > packet_two + def test_checksum(self): + self.assertEqual(Packet._calculate_checksum(b"abcde"), 0xF0C8) + self.assertEqual(Packet._calculate_checksum(b"abcdefgh"), 0x2706) + self.assertEqual( + Packet._calculate_checksum(b"abcdeabcdeabcdeabcdeabcde"), + 0xBAF4, + ) + if __name__ == "__main__": packet = TestPacket(False, 42, 3.14)