Implement the Simple Vehicle Dynamics Interface in C++

firmware/projects/VD_Interface/CMakeLists.txt

@@ -0,0 +1,12 @@
+# Teghveer Singh Ateliey
+# January 4, 2025
+
+# The target executable 'main' is created in the master CMakeLists. Do not change its name.
+# We only need to add the source code files and include directories.
+
+# include("${CMAKE_SOURCE_DIR}/cmake/cangen.cmake")
+
+target_sources(main PRIVATE main.cc)
+
+# Notice that we don't include any mcal/ subdirectory in this CMake file.
+# The master CMakeLists handles platform selection and library linking.

firmware/projects/VD_Interface/inc/simp_vd_interface.cc

@@ -0,0 +1,47 @@
+/// @author Teghveer Singh Ateliey
+/// @date 2024-11-23
+
+#include "simp_vd_interface.hpp"
+
+using namespace ctrl;
+
+SimpVdInterface::SimpVdInterface(
+    const shared::util::Mapper<float>& pedal_to_torque, float target_slip)
+    : pedal_to_torque(pedal_to_torque), target_slip(target_slip) {}
+
+VdOutput SimpVdInterface::update(const VdInput& input, int time_ms) {
+    VdOutput output{
+        .lm_torque_limit_positive = 0.0f,
+        .rm_torque_limit_positive = 0.0f,
+        .lm_torque_limit_negative = 0.0f, // negative limit fields set to
+        .rm_torque_limit_negative = 0.0f, // 0 in simulink model
+        .left_motor_speed_request = 1000,
+        .right_motor_speed_request = 1000,
+    };
+
+    float actual_slip =
+        CalculateActualSlip(input.wheel_speed_lr, input.wheel_speed_rr,
+                            input.wheel_speed_lf, input.wheel_speed_rf);
+    float tc_scale_factor = CalculateTCScaleFactor(actual_slip, target_slip, time_ms);
+
+    TorqueVector<float> torque_vector;
+    if (input.tv_enable) {
+        torque_vector = AdjustTorqueVectoring(input.steering_angle);
+    } else {
+        torque_vector.left_torque_vector = 1.0f;
+        torque_vector.right_torque_vector = 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);
+
+    output.lm_torque_limit_positive = motor_torque.left_motor_torque_limit;
+    output.rm_torque_limit_positive = motor_torque.right_motor_torque_limit;
+
+    return output;
+}

firmware/projects/VD_Interface/inc/simp_vd_interface.hpp

@@ -0,0 +1,44 @@
+/// @author Teghveer Singh Ateliey
+/// @date 2024-11-23
+
+#pragma once
+
+#include <cstdint>
+
+#include "shared/controls/motor_torque.h"
+#include "shared/controls/tc_scale_factor.h"
+#include "shared/controls/tvFactor.h"
+#include "shared/util/mappers/lookup_table.hpp"
+#include "shared/util/mappers/mapper.hpp"
+
+struct VdInput {
+    float driver_torque_request;
+    float brake_pedal_postion;
+    float steering_angle;
+    float wheel_speed_lr;
+    float wheel_speed_rr;
+    float wheel_speed_lf;
+    float wheel_speed_rf;
+    bool tv_enable;
+};
+
+struct VdOutput {
+    float lm_torque_limit_positive;
+    float rm_torque_limit_positive;
+    float lm_torque_limit_negative;
+    float rm_torque_limit_negative;
+    uint16_t left_motor_speed_request;
+    uint16_t right_motor_speed_request;
+};
+
+class SimpVdInterface {
+public:
+    SimpVdInterface(
+        const shared::util::Mapper<float>& pedal_to_torque,
+        float target_slip = 0.2f);  // default target slip is float 0.2
+    VdOutput update(const VdInput& input, int time_ms);
+
+private:
+    const shared::util::Mapper<float>& pedal_to_torque;
+    float target_slip;
+};

firmware/projects/VD_Interface/inc/simp_vd_interface_test.cc

@@ -0,0 +1,215 @@
+/// @author Teghveer Singh Ateliey
+/// @date 2024-11-23
+
+#include "simp_vd_interface.hpp"
+#include "shared/controls/testing.h"
+
+#include <cassert>
+#include <iostream>
+
+void test_1(const shared::util::Mapper<float>& pedal_to_torque) {
+    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(
+        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_1_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 1: (negative torque limits and speed requests are always constant)" << std::endl;
+    std::cout << "lm_torque_limit_positive: " << output_1.lm_torque_limit_positive << std::endl;
+    std::cout << "rm_torque_limit_positive: " << output_1.rm_torque_limit_positive << std::endl;
+
+    ASSERT_CLOSE(output_1.lm_torque_limit_positive, output_1_expected.lm_torque_limit_positive);
+    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) {
+    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(
+        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_2_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 1
+        .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 2: (negative torque limits and speed requests are always constant)" << std::endl;
+    std::cout << "lm_torque_limit_positive: " << output_2.lm_torque_limit_positive << std::endl;
+    std::cout << "rm_torque_limit_positive: " << output_2.rm_torque_limit_positive << std::endl;
+
+    ASSERT_CLOSE(output_2.lm_torque_limit_positive, output_2_expected.lm_torque_limit_positive);
+    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) {
+    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(
+        VdInput{
+            .driver_torque_request = 100.0f,
+            .brake_pedal_postion = 0.0f,
+            .steering_angle = 25.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_3_expected{
+        // Expected to calculate lm=100.0f, rm=68.3f torque limits given test 3 input
+        // Expected output of lm=66.7, rm=66.7*.683=45.5f after accounting for running avg of motor torque and prev tests
+        .lm_torque_limit_positive = 66.66667f,
+        .rm_torque_limit_positive = 45.53336f, // 5 decimal places due to tolerance
+        .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 3: (negative torque limits and speed requests are always constant)" << std::endl;
+    std::cout << "lm_torque_limit_positive: " << output_3.lm_torque_limit_positive << std::endl;
+    std::cout << "rm_torque_limit_positive: " << output_3.rm_torque_limit_positive << std::endl;
+
+    ASSERT_CLOSE(output_3.lm_torque_limit_positive, output_3_expected.lm_torque_limit_positive);
+    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) {
+    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(
+        VdInput{
+            .driver_torque_request = 50.0f,
+            .brake_pedal_postion = 0.0f,
+            .steering_angle = 0.0f,
+            .wheel_speed_lr = 15.0f, // actual slip 0.5
+            .wheel_speed_rr = 15.0f,
+            .wheel_speed_lf = 10.0f,
+            .wheel_speed_rf = 10.0f,
+            .tv_enable = true
+        },
+        time_ms);
+
+    VdOutput output_4_expected{
+        // Expected to calculate lm=rm=50.0f torque limit given test 4 input
+        // Moving avg: (prev torque limits) / count = (0 + 100 + 100 + 50) / 4 = 62.5f
+        .lm_torque_limit_positive = 62.5f,
+        .rm_torque_limit_positive = 62.5f, // tc scale factor = 1, slipping for < 50ms
+        .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 4: (negative torque limits and speed requests are always constant)" << std::endl;
+    std::cout << "lm_torque_limit_positive: " << output_4.lm_torque_limit_positive << std::endl;
+    std::cout << "rm_torque_limit_positive: " << output_4.rm_torque_limit_positive << std::endl;
+
+    ASSERT_CLOSE(output_4.lm_torque_limit_positive, output_4_expected.lm_torque_limit_positive);
+    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) {
+    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(
+        VdInput{
+            .driver_torque_request = 50.0f,
+            .brake_pedal_postion = 0.0f,
+            .steering_angle = 0.0f,
+            .wheel_speed_lr = 15.0f, // actual slip 0.5
+            .wheel_speed_rr = 15.0f,
+            .wheel_speed_lf = 10.0f,
+            .wheel_speed_rf = 10.0f,
+            .tv_enable = true
+        },
+        time_ms);
+
+    VdOutput output_5_expected{
+        // Expected to calculate lm=rm=0.0f torque limit given test 5 input
+        // Moving avg: (prev torque limits) / count = (0 + 100 + 100 + 50 + 0) / 5 = 50.0f
+        .lm_torque_limit_positive = 50.0f,
+        .rm_torque_limit_positive = 50.0f, // tc scale factor = 0, slipping for > 50ms
+        .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 5: (negative torque limits and speed requests are always constant)" << std::endl;
+    std::cout << "lm_torque_limit_positive: " << output_5.lm_torque_limit_positive << std::endl;
+    std::cout << "rm_torque_limit_positive: " << output_5.rm_torque_limit_positive << std::endl;
+
+    ASSERT_CLOSE(output_5.lm_torque_limit_positive, output_5_expected.lm_torque_limit_positive);
+    ASSERT_CLOSE(output_5.rm_torque_limit_positive, output_5_expected.rm_torque_limit_positive);
+}
+
+int start_tests() {
+    const float pedal_torque_lut_data[][2] = {
+        {0.0f, 0.0f}, 
+        {100.0f, 100.0f}
+    };
+    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);
+
+    std::cout << "Testing done!" << std::endl;
+
+    return 0;
+}

firmware/projects/VD_Interface/main.cc

@@ -0,0 +1,12 @@
+/// @author Teghveer Singh Ateliey
+/// @date 2025-01-04
+
+#include "inc/simp_vd_interface_test.cc"
+#include "inc/simp_vd_interface.hpp"
+#include "inc/simp_vd_interface.cc"
+
+int main(void) {
+    start_tests();
+
+    return 0;
+}

firmware/projects/VD_Interface/platforms/cli/CMakeLists.txt

@@ -0,0 +1,2 @@
+target_sources(main PRIVATE bindings.cc)
+target_link_libraries(main PRIVATE mcal-cli)

firmware/projects/VD_Interface/platforms/cli/bindings.cc

@@ -0,0 +1,15 @@
+/// @author Teghveer Singh Ateliey
+/// @date 2025-01-04
+
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <string>
+#include <thread>
+
+namespace bindings {
+void Initialize() {
+    std::cout << "Initializing CLI..." << std::endl;
+
+}
+}  // namespace bindings

firmware/projects/VD_Interface/platforms/cli/mcal_conf.cmake

@@ -0,0 +1 @@
+set(MCAL cli)