adding in safety check to drivetrain system for over-limit torque limit

hytech-racing · Oct 20, 2024 · 9deea29 · 9deea29
1 parent 5f887ed
commit 9deea29
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 8 deletions.
diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h
@@ -15,8 +15,8 @@ class DrivetrainSystem
 public:
     /// @brief order of array: 0: FL, 1: FR, 2: RL, 3: RR
     /// @param inverters inverter pointers
-    DrivetrainSystem(const std::array<InverterType *, 4> &inverters, MCUInterface *mcu_interface, int init_time_limit_ms, uint16_t min_hv_voltage = 60, int min_cmd_period_ms = 1)
-        : inverters_(inverters), init_time_limit_ms_(init_time_limit_ms), min_hv_voltage_(min_hv_voltage), min_cmd_period_(min_cmd_period_ms)
+    DrivetrainSystem(const std::array<InverterType *, 4> &inverters, MCUInterface *mcu_interface, int init_time_limit_ms, uint16_t min_hv_voltage = 60, int min_cmd_period_ms = 1, float max_torque_setpoint_nm = 21.42)
+        : inverters_(inverters), init_time_limit_ms_(init_time_limit_ms), min_hv_voltage_(min_hv_voltage), min_cmd_period_(min_cmd_period_ms), max_torque_setpoint_nm_(max_torque_setpoint_nm)
     {
         // values from: https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/fse/motor_data_sheet_a2370dd_dd5.pdf
         motor_pole_pairs_ = 5;
@@ -94,6 +94,7 @@ class DrivetrainSystem
     unsigned long drivetrain_initialization_phase_start_time_;
     DrivetrainCommand_s current_drivetrain_command_;
     DrivetrainDynamicReport_s dynamic_data_;
+    float max_torque_setpoint_nm_;
 };
 
 #include "DrivetrainSystem.tpp"

diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp
@@ -148,7 +148,15 @@ void DrivetrainSystem<InverterType>::command_drivetrain(const DrivetrainCommand_
         int index = 0;
         for (auto inv_pointer : inverters_)
         {
-            inv_pointer->handle_command({data.torqueSetpoints[index], data.speeds_rpm[index]});
+            float setpoint = 0;
+            if(data.torqueSetpoints[index] > max_torque_setpoint_nm_)
+            {
+                setpoint = max_torque_setpoint_nm_;
+            } else {
+                setpoint = data.torqueSetpoints[index];
+            }
+
+            inv_pointer->handle_command({setpoint, data.speeds_rpm[index]});
             index++;
         }
         // last_general_cmd_time_ = curr_system_millis_;

diff --git a/test/test_systems/drivetrain_system_test.h b/test/test_systems/drivetrain_system_test.h
@@ -179,17 +179,17 @@ TEST(DrivetrainSystemTesting, test_drivetrain_inverter_comms)
     SysClock clock;
     auto micros = 1000000;
     dt.tick(clock.tick(micros));
-    dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}});
+    dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {10.0, 11.0, 12.0, 13.0}});
     EXPECT_EQ(inv_fl.speed_setpoint_rpm_, 1000.0);
-    EXPECT_EQ(inv_fl.torque_setpoint_nm_, 2000.0);
+    EXPECT_EQ(inv_fl.torque_setpoint_nm_, 10.0);
 
-    EXPECT_EQ(inv_fr.torque_setpoint_nm_, 2001.0);
+    EXPECT_EQ(inv_fr.torque_setpoint_nm_, 11.0);
     EXPECT_EQ(inv_fr.speed_setpoint_rpm_, 1001.0);
 
-    EXPECT_EQ(inv_rl.torque_setpoint_nm_, 2002.0);
+    EXPECT_EQ(inv_rl.torque_setpoint_nm_, 12.0);
     EXPECT_EQ(inv_rl.speed_setpoint_rpm_, 1002.0);
 
-    EXPECT_EQ(inv_rr.torque_setpoint_nm_, 2003.0);
+    EXPECT_EQ(inv_rr.torque_setpoint_nm_, 13.0);
     EXPECT_EQ(inv_rr.speed_setpoint_rpm_, 1003.0);
 
     // testing to ensure that these extra general commands dont get through the period filter