Moved running average calculation to simp vd class

firmware/projects/VD_Interface/inc/simp_vd_interface.cc

@@ -31,17 +31,15 @@ VdOutput SimpVdInterface::update(const VdInput& input, int time_ms) {
         torque_vector.left = 1.0f;
         torque_vector.right = 1.0f;
     }
-
+    
     float motor_torque_request = ComputeTorqueRequest(input.driver_torque_request,
                                                 input.brake_pedal_postion);
 
-    // Running avg calculation done within CalculateMotorTorque
-    MotorTorque<float> motor_torque = CalculateMotorTorque(
-        pedal_to_torque.Evaluate(motor_torque_request * tc_scale_factor),
-        torque_vector);
+    running_average.LoadValue(pedal_to_torque.Evaluate(motor_torque_request * tc_scale_factor));
+    float running_average_value = running_average.GetValue();
 
-    output.lm_torque_limit_positive = motor_torque.left_limit;
-    output.rm_torque_limit_positive = motor_torque.right_limit;
+    output.lm_torque_limit_positive = running_average_value * torque_vector.left;
+    output.rm_torque_limit_positive = running_average_value * torque_vector.right;
 
     return output;
 }

firmware/projects/VD_Interface/inc/simp_vd_interface.hpp

@@ -40,5 +40,6 @@ class SimpVdInterface {
 
 private:
     const shared::util::Mapper<float>& pedal_to_torque;
+    shared::util::MovingAverage<float, 10> running_average;
     float target_slip;
 };

firmware/projects/VD_Interface/inc/simp_vd_interface_test.cc

@@ -7,13 +7,12 @@
 #include <cassert>
 #include <iostream>
 
-void test_1(const shared::util::Mapper<float>& pedal_to_torque) {
+void test_1(const shared::util::Mapper<float>& pedal_to_torque, SimpVdInterface* simp_vd_int) {
     std::cout << "Test 1 loading" << std::endl;
 
-    SimpVdInterface simp_vd_int{pedal_to_torque};
     int time_ms = 0;
 
-    VdOutput output_1 = simp_vd_int.update(
+    VdOutput output_1 = simp_vd_int->update(
         VdInput{
             .driver_torque_request = 0.0f,
             .brake_pedal_postion = 0.0f,
@@ -43,13 +42,12 @@ void test_1(const shared::util::Mapper<float>& pedal_to_torque) {
     ASSERT_CLOSE(output_1.lm_torque_limit_positive, output_1_expected.lm_torque_limit_positive);
 }
 
-void test_2(const shared::util::Mapper<float>& pedal_to_torque) {
+void test_2(const shared::util::Mapper<float>& pedal_to_torque, SimpVdInterface* simp_vd_int) {
     std::cout << "Test 2 loading" << std::endl;
 
-    SimpVdInterface simp_vd_int{pedal_to_torque};
     int time_ms = 0;
 
-    VdOutput output_2 = simp_vd_int.update(
+    VdOutput output_2 = simp_vd_int->update(
         VdInput{
             .driver_torque_request = 100.0f,
             .brake_pedal_postion = 0.0f,
@@ -81,13 +79,12 @@ void test_2(const shared::util::Mapper<float>& pedal_to_torque) {
     ASSERT_CLOSE(output_2.lm_torque_limit_positive, output_2_expected.lm_torque_limit_positive);
 }
 
-void test_3(const shared::util::Mapper<float>& pedal_to_torque) {
+void test_3(const shared::util::Mapper<float>& pedal_to_torque, SimpVdInterface* simp_vd_int) {
     std::cout << "Test 3 loading" << std::endl;
 
-    SimpVdInterface simp_vd_int{pedal_to_torque};
     int time_ms = 0;
 
-    VdOutput output_3 = simp_vd_int.update(
+    VdOutput output_3 = simp_vd_int->update(
         VdInput{
             .driver_torque_request = 100.0f,
             .brake_pedal_postion = 0.0f,
@@ -119,13 +116,12 @@ void test_3(const shared::util::Mapper<float>& pedal_to_torque) {
     ASSERT_CLOSE(output_3.lm_torque_limit_positive, output_3_expected.lm_torque_limit_positive);
 }
 
-void test_4(const shared::util::Mapper<float>& pedal_to_torque) {
+void test_4(const shared::util::Mapper<float>& pedal_to_torque, SimpVdInterface* simp_vd_int) {
     std::cout << "Test 4 loading" << std::endl;
 
-    SimpVdInterface simp_vd_int{pedal_to_torque};
     int time_ms = 0;
 
-    VdOutput output_4 = simp_vd_int.update(
+    VdOutput output_4 = simp_vd_int->update(
         VdInput{
             .driver_torque_request = 50.0f,
             .brake_pedal_postion = 0.0f,
@@ -157,13 +153,12 @@ void test_4(const shared::util::Mapper<float>& pedal_to_torque) {
     ASSERT_CLOSE(output_4.rm_torque_limit_positive, output_4_expected.rm_torque_limit_positive);
 }
 
-void test_5(const shared::util::Mapper<float>& pedal_to_torque) {
+void test_5(const shared::util::Mapper<float>& pedal_to_torque, SimpVdInterface* simp_vd_int) {
     std::cout << "Test 5 loading" << std::endl;
 
-    SimpVdInterface simp_vd_int{pedal_to_torque};
     int time_ms = 55;
 
-    VdOutput output_5 = simp_vd_int.update(
+    VdOutput output_5 = simp_vd_int->update(
         VdInput{
             .driver_torque_request = 50.0f,
             .brake_pedal_postion = 0.0f,
@@ -195,6 +190,79 @@ void test_5(const shared::util::Mapper<float>& pedal_to_torque) {
     ASSERT_CLOSE(output_5.rm_torque_limit_positive, output_5_expected.rm_torque_limit_positive);
 }
 
+void test_6(const shared::util::Mapper<float>& pedal_to_torque, SimpVdInterface* simp_vd_int) {
+    std::cout << "Test 6 loading" << std::endl;
+
+    int time_ms = 0;
+
+    VdOutput output_6 = simp_vd_int->update(
+        VdInput{
+            .driver_torque_request = 0.0f,
+            .brake_pedal_postion = 0.0f,
+            .steering_angle = 0.0f,
+            .wheel_speed_lr = 0.0f,
+            .wheel_speed_rr = 0.0f,
+            .wheel_speed_lf = 0.0f,
+            .wheel_speed_rf = 0.0f,
+            .tv_enable = true
+        },
+        time_ms);
+
+    VdOutput output_6_expected{
+        .lm_torque_limit_positive = 0.0f,
+        .rm_torque_limit_positive = 0.0f,
+        .lm_torque_limit_negative = 0.0f,
+        .rm_torque_limit_negative = 0.0f,
+        .left_motor_speed_request = 1000,
+        .right_motor_speed_request = 1000
+    };
+
+    std::cout << "Output 6: (negative torque limits and speed requests are always constant)" << std::endl;
+    std::cout << "lm_torque_limit_positive: " << output_6.lm_torque_limit_positive << std::endl;
+    std::cout << "rm_torque_limit_positive: " << output_6.rm_torque_limit_positive << std::endl;
+
+    ASSERT_CLOSE(output_6.lm_torque_limit_positive, output_6_expected.lm_torque_limit_positive);
+    ASSERT_CLOSE(output_6.lm_torque_limit_positive, output_6_expected.lm_torque_limit_positive);
+}
+
+void test_7(const shared::util::Mapper<float>& pedal_to_torque, SimpVdInterface* simp_vd_int) {
+    std::cout << "Test 7 loading" << std::endl;
+
+    // Ensure traction control resets
+    int time_ms = 200;
+
+    VdOutput output_7 = simp_vd_int->update(
+        VdInput{
+            .driver_torque_request = 100.0f,
+            .brake_pedal_postion = 0.0f,
+            .steering_angle = 0.0f,
+            .wheel_speed_lr = 0.0f,
+            .wheel_speed_rr = 0.0f,
+            .wheel_speed_lf = 0.0f,
+            .wheel_speed_rf = 0.0f,
+            .tv_enable = true
+        },
+        time_ms);
+
+    VdOutput output_7_expected{
+        // Expected to calculate torque limit of 100.0f for both motors given test 2 input
+        // Expected output of 50.0f after accounting for running avg of motor torque and test 6
+        .lm_torque_limit_positive = 50.0f,
+        .rm_torque_limit_positive = 50.0f,
+        .lm_torque_limit_negative = 0.0f,
+        .rm_torque_limit_negative = 0.0f,
+        .left_motor_speed_request = 1000,
+        .right_motor_speed_request = 1000
+    };
+
+    std::cout << "Output 7: (negative torque limits and speed requests are always constant)" << std::endl;
+    std::cout << "lm_torque_limit_positive: " << output_7.lm_torque_limit_positive << std::endl;
+    std::cout << "rm_torque_limit_positive: " << output_7.rm_torque_limit_positive << std::endl;
+
+    ASSERT_CLOSE(output_7.lm_torque_limit_positive, output_7_expected.lm_torque_limit_positive);
+    ASSERT_CLOSE(output_7.lm_torque_limit_positive, output_7_expected.lm_torque_limit_positive);
+}
+
 int start_tests() {
     const float pedal_torque_lut_data[][2] = {
         {0.0f, 0.0f}, 
@@ -203,11 +271,18 @@ int start_tests() {
     constexpr int pedal_torque_lut_length = (sizeof(pedal_torque_lut_data)) / (sizeof(pedal_torque_lut_data[0]));
     const shared::util::LookupTable<pedal_torque_lut_length> pedal_to_torque{pedal_torque_lut_data};
 
-    test_1(pedal_to_torque);
-    test_2(pedal_to_torque);
-    test_3(pedal_to_torque);
-    test_4(pedal_to_torque);
-    test_5(pedal_to_torque);
+    SimpVdInterface simp_vd_int{pedal_to_torque};
+    // Below tests all use the same SimpVdInterface object with running avg
+    test_1(pedal_to_torque, &simp_vd_int);
+    test_2(pedal_to_torque, &simp_vd_int);
+    test_3(pedal_to_torque, &simp_vd_int);
+    test_4(pedal_to_torque, &simp_vd_int);
+    test_5(pedal_to_torque, &simp_vd_int);
+
+    SimpVdInterface simp_vd_int_2{pedal_to_torque};
+    // New object with reset running avg
+    test_6(pedal_to_torque, &simp_vd_int_2);
+    test_7(pedal_to_torque, &simp_vd_int_2);
 
     std::cout << "Testing done!" << std::endl;
 

firmware/shared/controls/motor_torque.h

@@ -12,32 +12,6 @@ enum class State {
     Stop,
 };
 
-template <typename T>
-struct MotorTorque {
-    T left_limit;
-    T right_limit;
-};
-
-template <typename T>
-MotorTorque<T> CalculateMotorTorque(T new_torque_value, TorqueVector<T> torque_vector, bool reset = false) {
-    MotorTorque<T> torque;
-
-    static shared::util::MovingAverage<T, 10> running_average;
-
-    if (reset) {
-        running_average = shared::util::MovingAverage<T, 10>();
-    }
-
-    running_average.LoadValue(new_torque_value);
-
-    T running_average_value = running_average.GetValue();
-
-    torque.right_limit = running_average_value * torque_vector.right;
-    torque.left_limit = running_average_value * torque_vector.left;
-
-    return torque;
-}
-
 template <typename T>
 T ComputeTorqueRequest(T driver_torque_request, T brake_pedal_position) {
     static State current_state = State::Stop;

firmware/shared/controls/motor_torque_test.cc

@@ -8,105 +8,6 @@
 int main() {
     using namespace ctrl;
 
-    // Test cases for CalculateMotorTorque
-    // Test Case 1: Smaller Sample Sizes
-    {
-        TorqueVector<double> tv = {
-            .left = 1.5,
-            .right = 1.2
-        };
-
-        double new_torque_values[] = {5.0, 10.0, 15.0};
-        for (int i = 0; i < 3; i++) {
-            CalculateMotorTorque(new_torque_values[i], tv, i == 0);
-        }
-        MotorTorque<double> motor_torque =
-            CalculateMotorTorque(20.0, tv);
-        ASSERT_CLOSE(motor_torque.right_limit, 18.75);
-        ASSERT_CLOSE(motor_torque.left_limit, 15.00);
-    }
-
-    {
-        TorqueVector<double> tv = {
-            .left = 1.1,
-            .right = 1.3
-        };
-
-        double new_torque_values[] = {2.0, 4.0};
-        for (int i = 0; i < 2; i++) {
-            CalculateMotorTorque(new_torque_values[i], tv, i == 0);
-        }
-        MotorTorque<double> motor_torque =
-            CalculateMotorTorque(6.0, tv);
-        ASSERT_CLOSE(motor_torque.right_limit, 4.40);
-        ASSERT_CLOSE(motor_torque.left_limit, 5.20);
-    }
-
-    // Test Case 2: Larger Sample Sizes
-    {
-        TorqueVector<double> tv = {
-            .left = 1.5,
-            .right = 1.2
-        };
-
-        for (double i = 1; i <= 9; i++) {
-            CalculateMotorTorque(i, tv, i == 1);
-        }
-        MotorTorque<double> motor_torque =
-            CalculateMotorTorque(10.0, tv);
-        ASSERT_CLOSE(motor_torque.right_limit, 8.25);
-        ASSERT_CLOSE(motor_torque.left_limit, 6.60);
-    }
-
-    // Test Case 3: Sample Size over 10 Entities
-    {
-        TorqueVector<double> tv = {
-            .left = 1.1,
-            .right = 1.3
-        };
-
-        double new_torque_values[] = {80, 21, 34, 43, 27, 47, 46,
-                                      21, 73, 12, 11, 11, 14};
-        for (int i = 0; i < 13; i++) {
-            CalculateMotorTorque(new_torque_values[i], tv, i == 0);
-        }
-        MotorTorque<double> motor_torque =
-            CalculateMotorTorque(12.0, tv);
-        ASSERT_CLOSE(motor_torque.right_limit, 27.4 * 1.1);
-        ASSERT_CLOSE(motor_torque.left_limit, 27.4 * 1.2);
-    }
-
-    // Test Case 4: Zero values in raw_torque_values
-    {
-        TorqueVector<double> tv = {
-            .left = 1.1,
-            .right = 1.2
-        };
-
-        double new_torque_values[] = {0.0, 0.0, 0.0};
-        for (int i = 0; i < 3; i++) {
-            CalculateMotorTorque(new_torque_values[i], tv, i == 0);
-        }
-        MotorTorque<double> motor_torque = 
-            CalculateMotorTorque(0.0, tv);
-        ASSERT_CLOSE(motor_torque.right_limit, 0.0);
-        ASSERT_CLOSE(motor_torque.left_limit, 0.0);
-    }
-
-    // Test Case 5: Single element in raw_torque_values
-    {
-        TorqueVector<double> tv = {
-            .left = 1.5,
-            .right = 1.2
-        };
-
-        double new_torque_value = 10.0;
-        MotorTorque<double> motor_torque =
-            CalculateMotorTorque(new_torque_value, tv, true);
-        ASSERT_CLOSE(motor_torque.right_limit, 15.00);
-        ASSERT_CLOSE(motor_torque.left_limit, 12.00);
-    }
-
     // Test cases for ComputeTorqueRequest
     ASSERT_CLOSE(ComputeTorqueRequest(30.0, 20.0),
                  0.0);  // Should return 0.0 due to State::Stop