11#include " UMDLoopCANProtocol.hpp"
22#include < cstdio>
3- #include < cstdlib >
3+ #include < optional >
44
55using namespace Protocol ;
66
@@ -14,203 +14,64 @@ static int g_failures = 0;
1414 } \
1515 } while (false )
1616
17- static void test_message_type_dispatch () {
18- CanFrame f;
19-
20- f.id = POWER_PCB_C ::ID ;
21- CHECK (getMessageType (f) == MessageType::POWER_PCB_C );
22-
23- f.id = SCIENCE_SERVO_PCB_C ::ID ;
24- CHECK (getMessageType (f) == MessageType::SCIENCE_SERVO_PCB_C );
25-
26- f.id = SCIENCE_SERVO_PCB_R ::ID ;
27- CHECK (getMessageType (f) == MessageType::SCIENCE_SERVO_PCB_R );
28-
29- f.id = SWERVE_PCB_C ::ID ;
30- CHECK (getMessageType (f) == MessageType::SWERVE_PCB_C );
31-
32- f.id = 0xDEAD ;
33- CHECK (getMessageType (f) == MessageType::Unknown);
34-
35- std::puts (" test_message_type_dispatch done" );
36- }
37-
38- static void test_power_pcb_flat_roundtrip () {
39- POWER_PCB_C out;
40- out.cmd = 0x11 ;
41-
42- CanFrame frame = encode (out);
43- CHECK (frame.id == POWER_PCB_C ::ID ); // 0x10
44- CHECK (frame.dlc == POWER_PCB_C ::DLC ); // 3
45- CHECK (frame.data [0 ] == 0x11 ); // cmd in byte 0
46-
47- POWER_PCB_C in = decode<POWER_PCB_C >(frame);
48- CHECK (in.cmd == 0x11 );
49-
50- std::puts (" test_power_pcb_flat_roundtrip done" );
51- }
52-
53- static void test_science_servo_velocity_roundtrip () {
54- for (uint8_t port = 0 ; port < 8 ; ++port) {
55- int16_t vel = static_cast <int16_t >(-500 + port * 150 ); // mix of negative and positive
56-
57- SCIENCE_SERVO_PCB_C_servo_velocity_target_t payload;
58- payload.port_id = port;
59- payload.servo_velocity_target = vel;
60-
61- CanFrame frame = encode (payload);
62-
63- CHECK (frame.id == SCIENCE_SERVO_PCB_C ::ID ); // 0x80
64- CHECK (frame.dlc == 3 );
65- CHECK (frame.data [0 ] == static_cast <uint8_t >(0x30 + port)); // cmd byte
66- // little-endian payload
67- CHECK (frame.data [1 ] == static_cast <uint8_t >(vel & 0xFF ));
68- CHECK (frame.data [2 ] == static_cast <uint8_t >((vel >> 8 ) & 0xFF ));
69-
70- // decode payload
71- auto decoded = decode<SCIENCE_SERVO_PCB_C_servo_velocity_target_t>(frame);
72- CHECK (decoded.port_id == port);
73- CHECK (decoded.servo_velocity_target == vel);
74- }
75- std::puts (" test_science_servo_velocity_roundtrip done" );
17+ static void test_servo_position_roundtrip () {
18+ Servo::PositionTarget target{180 };
19+ // ScienceServo, Port 3 -> CAN ID 0x80, Mux 0x20 + 3 = 0x23
20+ CanFrame frame = Servo::encode_position_target (Subsystem::ScienceServo, 3 , target);
21+
22+ CHECK (frame.id == 0x80 );
23+ CHECK (frame.data [0 ] == 0x23 );
24+
25+ auto decoded = Servo::decode_position_target (frame);
26+ CHECK (decoded.has_value ());
27+ CHECK (decoded->port == 3 );
28+ CHECK (decoded->data .value == 180 );
29+
30+ std::puts (" test_servo_position_roundtrip done" );
7631}
7732
78- static void test_science_servo_position_roundtrip () {
79- for (uint8_t port = 0 ; port < 8 ; ++port) {
80- int16_t pos = static_cast <int16_t >(port * 1000 - 3500 );
81-
82- SCIENCE_SERVO_PCB_C_servo_position_target_t payload;
83- payload.port_id = port;
84- payload.servo_position_target = pos;
85-
86- CanFrame frame = encode (payload);
87-
88- CHECK (frame.id == SCIENCE_SERVO_PCB_C ::ID );
89- CHECK (frame.dlc == 3 );
90- CHECK (frame.data [0 ] == static_cast <uint8_t >(0x20 + port));
91- CHECK (frame.data [1 ] == static_cast <uint8_t >(pos & 0xFF ));
92- CHECK (frame.data [2 ] == static_cast <uint8_t >((pos >> 8 ) & 0xFF ));
93-
94- auto decoded = decode<SCIENCE_SERVO_PCB_C_servo_position_target_t>(frame);
95- CHECK (decoded.port_id == port);
96- CHECK (decoded.servo_position_target == pos);
97- }
98- std::puts (" test_science_servo_position_roundtrip done" );
33+ static void test_servo_position_multi_pcb () {
34+ Servo::PositionTarget target{90 };
35+
36+ // Swerve, Port 0 -> CAN ID 0x140, Mux 0x20 + 0 = 0x20
37+ CanFrame frame_swerve = Servo::encode_position_target (Subsystem::Swerve, 0 , target);
38+ CHECK (frame_swerve.id == 0x140 );
39+ CHECK (frame_swerve.data [0 ] == 0x20 );
40+
41+ auto decoded_swerve = Servo::decode_position_target (frame_swerve);
42+ CHECK (decoded_swerve.has_value ());
43+ CHECK (decoded_swerve->port == 0 );
44+ CHECK (decoded_swerve->data .value == 90 );
45+
46+ // BaseArm, Port 2 -> CAN ID 0x130, Mux 0x20 + 2 = 0x22
47+ CanFrame frame_arm = Servo::encode_position_target (Subsystem::BaseArm, 2 , target);
48+ CHECK (frame_arm.id == 0x130 );
49+ CHECK (frame_arm.data [0 ] == 0x22 );
50+
51+ auto decoded_arm = Servo::decode_position_target (frame_arm);
52+ CHECK (decoded_arm.has_value ());
53+ CHECK (decoded_arm->port == 2 );
54+
55+ std::puts (" test_servo_position_multi_pcb done" );
9956}
10057
101- static void test_science_servo_bool_commands () {
102- for (uint8_t port = 0 ; port < 8 ; ++port) {
103- SCIENCE_SERVO_PCB_C_servo_state_req_event_t state_payload;
104- state_payload.port_id = port;
105- state_payload.servo_state_req_event = true ;
106- CanFrame state_frame = encode (state_payload);
107-
108- SCIENCE_SERVO_PCB_C_servo_status_req_event_t status_payload;
109- status_payload.port_id = port;
110- status_payload.servo_status_req_event = true ;
111- CanFrame status_frame = encode (status_payload);
112-
113- CHECK (state_frame.data [0 ] == static_cast <uint8_t >(0x40 + port));
114- CHECK (status_frame.data [0 ] == static_cast <uint8_t >(0x50 + port));
115-
116- auto state_decoded = decode<SCIENCE_SERVO_PCB_C_servo_state_req_event_t>(state_frame);
117- CHECK (state_decoded.port_id == port);
118- CHECK (state_decoded.servo_state_req_event == true );
119-
120- auto status_decoded = decode<SCIENCE_SERVO_PCB_C_servo_status_req_event_t>(status_frame);
121- CHECK (status_decoded.port_id == port);
122- CHECK (status_decoded.servo_status_req_event == true );
123- }
124- std::puts (" test_science_servo_bool_commands done" );
125- }
126-
127- static void test_science_servo_pos_response_roundtrip () {
128- for (uint8_t port = 0 ; port < 8 ; ++port) {
129- SCIENCE_SERVO_PCB_R_pos_resp_t payload;
130- payload.port_id = port;
131- payload.servo_position_pos_resp = static_cast <int16_t >(port * 200 - 700 );
132- payload.servo_velocity_pos_resp = static_cast <int16_t >(port * 50 - 100 );
133-
134- CanFrame frame = encode (payload);
135-
136- CHECK (frame.id == SCIENCE_SERVO_PCB_R ::ID ); // 0x81
137- CHECK (frame.dlc == 5 );
138- CHECK (frame.data [0 ] == static_cast <uint8_t >(0x20 + port));
139- // position little-endian at bytes 1-2
140- CHECK (frame.data [1 ] == static_cast <uint8_t >(payload.servo_position_pos_resp & 0xFF ));
141- CHECK (frame.data [2 ] == static_cast <uint8_t >((payload.servo_position_pos_resp >> 8 ) & 0xFF ));
142- // velocity little-endian at bytes 3-4
143- CHECK (frame.data [3 ] == static_cast <uint8_t >(payload.servo_velocity_pos_resp & 0xFF ));
144- CHECK (frame.data [4 ] == static_cast <uint8_t >((payload.servo_velocity_pos_resp >> 8 ) & 0xFF ));
145-
146- auto decoded = decode<SCIENCE_SERVO_PCB_R_pos_resp_t>(frame);
147- CHECK (decoded.port_id == port);
148- CHECK (decoded.servo_position_pos_resp == payload.servo_position_pos_resp );
149- CHECK (decoded.servo_velocity_pos_resp == payload.servo_velocity_pos_resp );
150- }
151- std::puts (" test_science_servo_pos_response_roundtrip done" );
152- }
153-
154- static void test_swerve_pcb_mux_commands () {
155- for (uint8_t port = 0 ; port < 2 ; ++port) {
156- int16_t pos = static_cast <int16_t >(port * 4500 - 2000 );
157- int16_t vel = static_cast <int16_t >(port * 300 - 100 );
158-
159- SWERVE_PCB_C_servo_position_target_t pos_payload{port, pos};
160- SWERVE_PCB_C_servo_velocity_target_t vel_payload{port, vel};
161-
162- CanFrame pos_frame = encode (pos_payload);
163- CanFrame vel_frame = encode (vel_payload);
164-
165- CHECK (pos_frame.id == SWERVE_PCB_C ::ID );
166- CHECK (vel_frame.id == SWERVE_PCB_C ::ID );
167-
168- auto pos_decoded = decode<SWERVE_PCB_C_servo_position_target_t>(pos_frame);
169- CHECK (pos_decoded.port_id == port);
170- CHECK (pos_decoded.servo_position_target == pos);
171-
172- auto vel_decoded = decode<SWERVE_PCB_C_servo_velocity_target_t>(vel_frame);
173- CHECK (vel_decoded.port_id == port);
174- CHECK (vel_decoded.servo_velocity_target == vel);
175- }
176- std::puts (" test_swerve_pcb_mux_commands done" );
177- }
178-
179- static void test_boundary_values () {
180- {
181- SCIENCE_SERVO_PCB_C_servo_velocity_target_t payload{0 , INT16_MIN };
182- CanFrame f = encode (payload);
183- auto decoded = decode<SCIENCE_SERVO_PCB_C_servo_velocity_target_t>(f);
184- CHECK (decoded.servo_velocity_target == INT16_MIN );
185- }
186- {
187- SCIENCE_SERVO_PCB_C_servo_velocity_target_t payload{7 , INT16_MAX };
188- CanFrame f = encode (payload);
189- auto decoded = decode<SCIENCE_SERVO_PCB_C_servo_velocity_target_t>(f);
190- CHECK (decoded.servo_velocity_target == INT16_MAX );
191- CHECK (decoded.port_id == 7 );
192- }
193- // zero
194- {
195- SCIENCE_SERVO_PCB_C_servo_position_target_t payload{3 , 0 };
196- CanFrame f = encode (payload);
197- auto decoded = decode<SCIENCE_SERVO_PCB_C_servo_position_target_t>(f);
198- CHECK (decoded.servo_position_target == 0 );
199- CHECK (f.data [1 ] == 0 );
200- CHECK (f.data [2 ] == 0 );
201- }
202- std::puts (" test_boundary_values done" );
58+ static void test_dc_motor_status () {
59+ DcMotor::Status stat{1 };
60+
61+ CanFrame frame = DcMotor::encode_status (Subsystem::ScienceDcMotor, 0 , stat);
62+ CHECK (frame.id == 0x71 );
63+
64+ auto decoded = DcMotor::decode_status (frame);
65+ CHECK (decoded.has_value ());
66+ CHECK (decoded->data .value == 1 );
67+
68+ std::puts (" test_dc_motor_status done" );
20369}
20470
20571int main () {
206- test_message_type_dispatch ();
207- test_power_pcb_flat_roundtrip ();
208- test_science_servo_velocity_roundtrip ();
209- test_science_servo_position_roundtrip ();
210- test_science_servo_bool_commands ();
211- test_science_servo_pos_response_roundtrip ();
212- test_swerve_pcb_mux_commands ();
213- test_boundary_values ();
72+ test_servo_position_roundtrip ();
73+ test_servo_position_multi_pcb ();
74+ test_dc_motor_status ();
21475
21576 if (g_failures == 0 ) {
21677 std::puts (" \n All tests passed." );
0 commit comments