Merge branch 'master' into tc_mux_testing

.github/workflows/docs.yml

@@ -17,6 +17,10 @@ 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

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

lib/systems/include/TorqueControllerMux.h

@@ -75,6 +75,7 @@ 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);

test/test_systems/tc_mux_v2_testing.h

@@ -4,7 +4,6 @@
 #include "TorqueControllerMux.h"
 #include "TorqueControllers.h"
 #include "fake_controller_type.h"
-#include <gtest/gtest.h>
 
 
 
@@ -50,30 +49,6 @@ TEST(TorqueControllerMuxTesting, test_construction)
     TestControllerType inst1, inst2;
     TorqueControllerMux<2> test({static_cast<Controller *>(&inst1), static_cast<Controller *>(&inst2)}, {false, false});
 }
-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<DummyQueue_s> case_sys(&q, 100, 70, 550, {});
-    TorqueControllerCASEWrapper<DummyQueue_s> case_wrapper(&case_sys);
-
-    // mode 3
-    TorqueControllerSimpleLaunch simple_launch;
-    // mode 4
-    TorqueControllerSlipLaunch slip_launch;
-
-    TorqueControllerMux<5>
-        torque_controller_mux({static_cast<Controller *>(&tc_simple),
-                               static_cast<Controller *>(&tc_vec),
-                               static_cast<Controller *>(&case_wrapper),
-                               static_cast<Controller *>(&simple_launch),
-                               static_cast<Controller *>(&slip_launch)},
-                              {false, false, true, false, false});
-}
 TEST(TorqueControllerMuxTesting, test_construction_bypass_limits)
 {
     TestControllerType inst1, inst2;
@@ -108,6 +83,21 @@ TEST(TorqueControllerMuxTesting, test_construction_bypass_limits)
 
     ASSERT_EQ(test.get_tc_mux_status().output_is_bypassing_limits, true);
 }
+TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error)
+{
+    TestControllerType inst1, inst2;
+    TorqueControllerMux<2> test({static_cast<Controller *>(&inst1), static_cast<Controller *>(&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);
+    }
+}
 template<int num_controllers>
 void test_with_n_controllers() {
     SharedCarState_s state({}, {}, {}, {}, {}, {});
@@ -128,7 +118,6 @@ void test_with_n_controllers() {
     // Assert no error has occurred
     ASSERT_EQ(mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR);
 }
-
 // Test case for different numbers of controllers (hardcoded bc needs a const)
 TEST(TorqueControllerMuxTesting, test_variable_length_controllers)
 {
@@ -143,6 +132,30 @@ TEST(TorqueControllerMuxTesting, test_variable_length_controllers)
     test_with_n_controllers<9>();
     test_with_n_controllers<10>();
 }
+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<DummyQueue_s> case_sys(&q, 100, 70, 550, {});
+    TorqueControllerCASEWrapper<DummyQueue_s> case_wrapper(&case_sys);
+
+    // mode 3
+    TorqueControllerSimpleLaunch simple_launch;
+    // mode 4
+    TorqueControllerSlipLaunch slip_launch;
+
+    TorqueControllerMux<5>
+        torque_controller_mux({static_cast<Controller *>(&tc_simple),
+                               static_cast<Controller *>(&tc_vec),
+                               static_cast<Controller *>(&case_wrapper),
+                               static_cast<Controller *>(&simple_launch),
+                               static_cast<Controller *>(&slip_launch)},
+                              {false, false, true, false, false});
+}
 
 //logic testing
 // ensure that swapping to a controller that has a higher desired output speed than previously
@@ -155,24 +168,24 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic)
     TorqueControllerMux<2> test({static_cast<Controller *>(&inst1), static_cast<Controller *>(&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);
 }
@@ -265,24 +278,24 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit)
     set_outputs(inst2, 3, 10);
     TorqueControllerMux<2> test({static_cast<Controller *>(&inst1), static_cast<Controller *>(&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);
     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);
     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);
@@ -320,13 +333,11 @@ TEST(TorqueControllerMuxTesting, test_speed_diff_swap_limit)
     ASSERT_EQ(out1.torqueSetpoints[1], 1);
     ASSERT_EQ(out1.torqueSetpoints[2], 1);
     ASSERT_EQ(out1.torqueSetpoints[3], 1);
-}
-
-//make generic
-//mode evalutaion testing
+
+//Mode evaluation tests
 TEST(TorqueControllerMuxTesting, test_mode0_evaluation)
 {
- 
+
     float max_torque = 21.42;
     // mode 0
     TorqueControllerSimple tc_simple(1.0f, 1.0f);
@@ -336,7 +347,7 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation)
     DummyQueue_s q;
     CASESystem<DummyQueue_s> case_sys(&q, 100, 70, 550, {});
     TorqueControllerCASEWrapper<DummyQueue_s> case_wrapper(&case_sys);
- 
+
     // mode 3
     TorqueControllerSimpleLaunch simple_launch;
     // mode 4
@@ -348,39 +359,39 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation)
                                                   static_cast<Controller *>(&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
+
+ //limit testing
 TEST(TorqueControllerMuxTesting, test_power_limit)
 {
     TestControllerType inst1;
     set_output_rpm(inst1, 20000, 10.0);
     TorqueControllerMux<1> test({static_cast<Controller *>(&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);
@@ -390,49 +401,50 @@ 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<Controller *>(&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]);
@@ -524,5 +536,4 @@ TEST(TorqueControllerMuxTesting, test_real_controllers_output)
     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__