diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index fe7ed526b..d3a5d166a 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -17,6 +17,9 @@ jobs: uses: mattnotmitt/doxygen-action@v1.9.5 with: doxyfile-path: 'Doxyfile' + + - name: Install dependencies + run: sudo apt-get update && sudo apt-get install -y graphviz - name: Deploy to GitHub Pages uses: peaceiris/actions-gh-pages@v3 diff --git a/Doxyfile b/Doxyfile index 94c359474..3cf55d9e7 100644 --- a/Doxyfile +++ b/Doxyfile @@ -2481,7 +2481,7 @@ HIDE_UNDOC_RELATIONS = YES # set to NO # The default value is: NO. -HAVE_DOT = NO +HAVE_DOT = YES # The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed # to run in parallel. When set to 0 doxygen will base this on the number of @@ -2565,7 +2565,7 @@ GROUP_GRAPHS = YES # The default value is: NO. # This tag requires that the tag HAVE_DOT is set to YES. -UML_LOOK = NO +UML_LOOK = YES # If the UML_LOOK tag is enabled, the fields and methods are shown inside the # class node. If there are many fields or methods and many nodes the graph may diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 1db0cfb80..21d89efb2 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -75,7 +75,6 @@ class TorqueControllerMux TorqueControllerMuxError can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, DrivetrainCommand_s previous_controller_command, DrivetrainCommand_s desired_controller_out); - DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 7d7d1beae..086fc108b 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -4,6 +4,7 @@ #include "TorqueControllerMux.h" #include "TorqueControllers.h" #include "fake_controller_type.h" +#include @@ -42,29 +43,73 @@ void set_output_rpm(controller_type &controller, float rpm, float torque) controller.output.command.torqueSetpoints[2] = torque; controller.output.command.torqueSetpoints[3] = torque; } + +//construction testing TEST(TorqueControllerMuxTesting, test_construction) { TestControllerType inst1, inst2; TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); } - -TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) +TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) +{ + // mode 0 + TorqueControllerSimple tc_simple(1.0f, 1.0f); + // mode 1 + TorqueControllerLoadCellVectoring tc_vec; + // mode 2 + DummyQueue_s q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); + + // mode 3 + TorqueControllerSimpleLaunch simple_launch; + // mode 4 + TorqueControllerSlipLaunch slip_launch; + + TorqueControllerMux<5> + torque_controller_mux({static_cast(&tc_simple), + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&slip_launch)}, + {false, false, true, false, false}); +} +TEST(TorqueControllerMuxTesting, test_construction_bypass_limits) { TestControllerType inst1, inst2; - TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); - SharedCarState_s state({}, {}, {}, {}, {}, {}); - auto res = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS); - for (int i =0; i< 4; i++) + set_output_rpm(inst1, 500, 30.0); + set_output_rpm(inst2, 500, 30.0); + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {true, false}); + + DrivetrainDynamicReport_s drivetrain_data = {}; + for (int i = 0; i < 4; i++) { - - ASSERT_EQ(res.speeds_rpm[i], 0.0); - ASSERT_EQ(res.torqueSetpoints[i], 0.0); + drivetrain_data.measuredSpeeds[i] = 500.0f; } + //onlhy use mode 0 for the input state in all tests and then test if they would act the same, idk how to do that + SharedCarState_s mode_0_input_state({}, {}, drivetrain_data, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); + + auto drive_command = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, mode_0_input_state); + + ASSERT_EQ(drive_command.torqueSetpoints[0], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[1], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[2], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[3], 30.0f); + + ASSERT_EQ(test.get_tc_mux_status().output_is_bypassing_limits, true); + + inst1.output.command.torqueSetpoints[0] = 5; + drive_command = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, mode_0_input_state); + + ASSERT_EQ(drive_command.torqueSetpoints[0], 5.0f); + ASSERT_EQ(drive_command.torqueSetpoints[1], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[2], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[3], 30.0f); + + ASSERT_EQ(test.get_tc_mux_status().output_is_bypassing_limits, true); } - - + +//logic testing // ensure that swapping to a controller that has a higher desired output speed than previously // commanded that we dont switch TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) @@ -75,28 +120,82 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); SharedCarState_s state({}, {}, {}, {}, {}, {}); set_four_outputs(state.drivetrain_data.measuredSpeeds, 10000.0); - + state.pedals_data = {}; state.vn_data = {}; - + auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - + // auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH); - + set_outputs(inst1, 0, 1); set_outputs(inst2, 0, 1); set_four_outputs(state.drivetrain_data.measuredSpeeds, 0); - + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_1); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); } - +TEST(TorqueControllerMuxTesting, simple_switch_many_active_modes) +{ + TestControllerType inst1, inst2, inst3, inst4; + set_outputs(inst1, 0, 1); + set_outputs(inst2, 0, 1); + set_outputs(inst3, 0, 1); + set_outputs(inst4, 0, 1); + TorqueControllerMux<4> test({static_cast(&inst1), static_cast(&inst2), static_cast(&inst3), static_cast(&inst4)}, {false, false, false, false}); + SharedCarState_s state({}, {}, {}, {}, {}, {}); + set_four_outputs(state.drivetrain_data.measuredSpeeds, 2); + + state.pedals_data = {}; + state.vn_data = {}; + + auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_1); + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_3); + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); +} + +//TorqueControllerMuxError testing +TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) +{ + TestControllerType inst1, inst2; + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); + SharedCarState_s state({}, {}, {}, {}, {}, {}); + auto res = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS); + for (int i =0; i< 4; i++) + { + + ASSERT_EQ(res.speeds_rpm[i], 0.0); + ASSERT_EQ(res.torqueSetpoints[i], 0.0); + } +} TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) { TestControllerType inst1, inst2; @@ -104,12 +203,12 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) set_outputs(inst2, 3, 10); TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); SharedCarState_s state({}, {}, {}, {}, {}, {}); - + auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); - + // tick it a bunch of times out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); @@ -117,45 +216,35 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); - + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); - + ASSERT_EQ(out1.torqueSetpoints[0], 1); ASSERT_EQ(out1.torqueSetpoints[1], 1); ASSERT_EQ(out1.torqueSetpoints[2], 1); ASSERT_EQ(out1.torqueSetpoints[3], 1); } - -TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) +TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) { - // mode 0 - TorqueControllerSimple tc_simple(1.0f, 1.0f); - // mode 1 - TorqueControllerLoadCellVectoring tc_vec; - // mode 2 - DummyQueue_s q; - CASESystem case_sys(&q, 100, 70, 550, {}); - TorqueControllerCASEWrapper case_wrapper(&case_sys); - - // mode 3 - TorqueControllerSimpleLaunch simple_launch; - // mode 4 - TorqueControllerSlipLaunch slip_launch; - - TorqueControllerMux<5> - torque_controller_mux({static_cast(&tc_simple), - static_cast(&tc_vec), - static_cast(&case_wrapper), - static_cast(&simple_launch), - static_cast(&slip_launch)}, - {false, false, true, false, false}); + TorqueControllerMux<1> test({nullptr}, {true}); + SharedCarState_s state({}, {}, {}, {}, {}, {}); + auto res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, state); + for (int i = 0; i < 4; i++) + { + ASSERT_EQ(res.torqueSetpoints[i], 0.0f); + ASSERT_EQ(res.speeds_rpm[i], 0.0f); + } + //makes it not able to test, assume i didnt pull most recent code + // ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); } - + +//make generic +//mode evalutaion testing TEST(TorqueControllerMuxTesting, test_mode0_evaluation) { - + float max_torque = 21.42; // mode 0 TorqueControllerSimple tc_simple(1.0f, 1.0f); @@ -165,7 +254,7 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) DummyQueue_s q; CASESystem case_sys(&q, 100, 70, 550, {}); TorqueControllerCASEWrapper case_wrapper(&case_sys); - + // mode 3 TorqueControllerSimpleLaunch simple_launch; // mode 4 @@ -177,38 +266,39 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) static_cast(&slip_launch)}, {false, true, false, false, false}); SharedCarState_s mode_0_input_state({}, {}, {}, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); - + DrivetrainCommand_s out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); ASSERT_NEAR(out.torqueSetpoints[0], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[1], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[2], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[3], (max_torque / 2), 0.01); - + mode_0_input_state = {{}, {}, {}, {}, {.accelPercent = 0.0f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}}; out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); ASSERT_EQ(out.torqueSetpoints[0], 0); ASSERT_EQ(out.torqueSetpoints[1], 0); ASSERT_EQ(out.torqueSetpoints[2], 0); ASSERT_EQ(out.torqueSetpoints[3], 0); - + // out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_1_input_state); } - + +//limit testing TEST(TorqueControllerMuxTesting, test_power_limit) { TestControllerType inst1; set_output_rpm(inst1, 20000, 10.0); TorqueControllerMux<1> test({static_cast(&inst1)}, {false}); - + DrivetrainDynamicReport_s drivetrain_data = {}; for (int i = 0; i < 4; i++) { drivetrain_data.measuredSpeeds[i] = 500.0f; } SharedCarState_s mode_0_input_state({}, {}, drivetrain_data, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); - + DrivetrainCommand_s res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); - + for (int i = 0; i < 4; i++) { ASSERT_EQ(res.torqueSetpoints[i], 10.0f); @@ -218,66 +308,138 @@ TEST(TorqueControllerMuxTesting, test_power_limit) drivetrain_data.measuredSpeeds[i] = 20000.0f; } set_output_rpm(inst1, 20000, 21.0); - + SharedCarState_s mode_0_input_state_high_power({}, {}, drivetrain_data, {}, {.accelPercent = 1.0f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state_high_power); - + for (int i = 0; i < 4; i++) { ASSERT_LT(res.torqueSetpoints[i], 7.6); // hardcoded value based on online calculator } } - TEST(TorqueControllerMuxTesting, test_torque_limit) { - + TestControllerType inst1; - + set_output_rpm(inst1, 500, 10.0); inst1.output.command.torqueSetpoints[0] = 5; TorqueControllerMux<1> test({static_cast(&inst1)}, {false}); - + DrivetrainDynamicReport_s drivetrain_data = {}; for (int i = 0; i < 4; i++) { drivetrain_data.measuredSpeeds[i] = 500.0f; } - + SharedCarState_s mode_0_input_state({}, {}, drivetrain_data, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); - + auto drive_command = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, mode_0_input_state); - + ASSERT_EQ(drive_command.torqueSetpoints[0], 5.0f); ASSERT_EQ(drive_command.torqueSetpoints[1], 10.0f); ASSERT_EQ(drive_command.torqueSetpoints[2], 10.0f); ASSERT_EQ(drive_command.torqueSetpoints[3], 10.0f); - + set_output_rpm(inst1, 500, 20.0); inst1.output.command.torqueSetpoints[0] = 5; - + drive_command = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, mode_0_input_state); - + ASSERT_LT(drive_command.torqueSetpoints[0], 3.5f); ASSERT_LT(drive_command.torqueSetpoints[1], 12.5f); ASSERT_LT(drive_command.torqueSetpoints[2], 12.5f); ASSERT_LT(drive_command.torqueSetpoints[3], 12.5f); - + printf("torque 1: %.2f\n", drive_command.torqueSetpoints[0]); printf("torque 2: %.2f\n", drive_command.torqueSetpoints[1]); printf("torque 3: %.2f\n", drive_command.torqueSetpoints[2]); printf("torque 4: %.2f\n", drive_command.torqueSetpoints[3]); } -TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) +//integration testing +TEST(TorqueControllerMuxTesting, test_tc_mux_status) { - TorqueControllerMux<1> test({nullptr}, {true}); + TorqueControllerMuxError testErr = TorqueControllerMuxError::NO_ERROR; + ControllerMode_e testMode = ControllerMode_e::MODE_0; + TorqueLimit_e testTorqLim = TorqueLimit_e::TCMUX_FULL_TORQUE; + TestControllerType inst1; + set_outputs(inst1, 0, 1); + TorqueControllerMux<1> test({static_cast(&inst1)}, { false }); SharedCarState_s state({}, {}, {}, {}, {}, {}); - auto res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, state); - for (int i = 0; i < 4; i++) - { - ASSERT_EQ(res.torqueSetpoints[i], 0.0f); - ASSERT_EQ(res.speeds_rpm[i], 0.0f); - } - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); + set_four_outputs(state.drivetrain_data.measuredSpeeds, 2); + + state.pedals_data = {}; + state.vn_data = {}; + + auto out1 = test.getDrivetrainCommand(testMode, testTorqLim, state); + ASSERT_EQ(test.get_tc_mux_status().current_error, testErr); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, testMode); + ASSERT_EQ(test.get_tc_mux_status().current_torque_limit_enum, testTorqLim); } +TEST(TorqueControllerMuxTesting, test_real_controllers_output) +{ + //mode 0 + TorqueControllerSimple tc_simple(1.0f, 1.0f); + // mode 1 + TorqueControllerLoadCellVectoring tc_vec; + // mode 2 + DummyQueue_s q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); + + // mode 3 + TorqueControllerSimpleLaunch simple_launch; + // mode 4 + TorqueControllerSlipLaunch slip_launch; + + TorqueControllerMux<5> + torque_controller_mux({static_cast(&tc_simple), + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&slip_launch)}, + {false, false, false, false, false}); + SharedCarState_s state({}, {}, {}, {}, {}, {}); + state.pedals_data = {}; + state.vn_data = {}; + state.drivetrain_data = {}; + + auto out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_1); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(static_cast(torque_controller_mux.get_tc_mux_status().current_error), static_cast(TorqueControllerMuxError::NO_ERROR)); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_3); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_4); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_4); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_3); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); +} + #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file