controls algo setup

firmware/CMakeLists.txt

@@ -1,7 +1,8 @@
 include(cmake/dependencies.cmake)
 include(../scripts/code_generation/commit_info_gen/commit_info.cmake)
-include(cmake/eigenlib.cmake)   
+include(cmake/eigenlib.cmake)
 include(cmake/autodiff.cmake)
+include(cmake/matplotlib.cmake)
 
 IF ("${TARGET}" STREQUAL "binary")
     option(BOOTLOAD "Build the bootloader" OFF)

firmware/cmake/dependencies.cmake

@@ -66,19 +66,19 @@ IF ("${TARGET}" STREQUAL "binary")
     )
     # Autodiff library: contains header only apis to automatically compute derrivatives of functions 
     CPMAddPackage(
-        NAME AUTO_DIFF
-        GITHUB_REPOSITORY autodiff/autodiff
-        GIT_TAG 2e2f3a2b16afcd9c04e76c8a689e9fd23ff78679
-        GIT_SHALLOW TRUE
-        DOWNLOAD_ONLY TRUE
+            NAME AUTO_DIFF
+            GITHUB_REPOSITORY autodiff/autodiff
+            GIT_TAG 2e2f3a2b16afcd9c04e76c8a689e9fd23ff78679
+            GIT_SHALLOW TRUE
+            DOWNLOAD_ONLY TRUE
     )
     # Eigen Library: provides support for matrices and matrix algebra 
     CPMAddPackage(
-        NAME EIGEN
-        GIT_REPOSITORY https://gitlab.com/libeigen/eigen
-        GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c
-        GIT_SHALLOW TRUE
-        DOWNLOAD_ONLY TRUE
+            NAME EIGEN
+            GIT_REPOSITORY https://gitlab.com/libeigen/eigen
+            GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c
+            GIT_SHALLOW TRUE
+            DOWNLOAD_ONLY TRUE
     )
 ELSEIF ("${TARGET}" STREQUAL "test")
     # Fetch GoogleTest for unit testing.
@@ -90,29 +90,37 @@ ELSEIF ("${TARGET}" STREQUAL "test")
     )
     # Autodiff library: contains header only apis to automatically compute derrivatives of functions
     CPMAddPackage(
-        NAME AUTO_DIFF
-        GITHUB_REPOSITORY autodiff/autodiff
-        GIT_TAG 2e2f3a2b16afcd9c04e76c8a689e9fd23ff78679
-        GIT_SHALLOW TRUE
-        DOWNLOAD_ONLY TRUE
+            NAME AUTO_DIFF
+            GITHUB_REPOSITORY autodiff/autodiff
+            GIT_TAG 2e2f3a2b16afcd9c04e76c8a689e9fd23ff78679
+            GIT_SHALLOW TRUE
+            DOWNLOAD_ONLY TRUE
     )
     # Eigen Library: provides support for matrices and matrix algebra
     CPMAddPackage(
-        NAME EIGEN
-        GIT_REPOSITORY https://gitlab.com/libeigen/eigen
-        GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c
-        GIT_SHALLOW TRUE
-        DOWNLOAD_ONLY TRUE
+            NAME EIGEN
+            GIT_REPOSITORY https://gitlab.com/libeigen/eigen
+            GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c
+            GIT_SHALLOW TRUE
+            DOWNLOAD_ONLY TRUE
     )
 
     # pybind 11: library for creating python bindings for C++ code
     CPMAddPackage(
-        NAME pybind11
-        GIT_REPOSITORY https://github.com/pybind/pybind11.git
-        VERSION 3.0.2
-        DOWNLOAD_ONLY TRUE
+            NAME pybind11
+            GIT_REPOSITORY https://github.com/pybind/pybind11.git
+            VERSION 3.0.2
+            DOWNLOAD_ONLY TRUE
     )
     add_subdirectory(${pybind11_SOURCE_DIR} ${pybind11_BINARY_DIR})
+
+    CPMAddPackage(
+            NAME matplotlibcpp
+            GITHUB_REPOSITORY lava/matplotlib-cpp
+            GIT_TAG ef0383f1315d32e0156335e10b82e90b334f6d9f
+            GIT_SHALLOW TRUE
+            DOWNLOAD_ONLY TRUE
+    )
 ENDIF ()
 
 # protobufs

firmware/cmake/matplotlib.cmake

@@ -0,0 +1,20 @@
+message("")
+message("Configuring matplotlib-cpp...")
+
+# Library target
+find_package(Python3 COMPONENTS Development REQUIRED)
+add_library(matplotlib_cpp INTERFACE)
+target_include_directories(matplotlib_cpp INTERFACE ${matplotlibcpp_SOURCE_DIR})
+target_link_libraries(matplotlib_cpp INTERFACE
+        Python3::Python
+        Python3::Module
+)
+#find_package(Python3 COMPONENTS NumPy)
+#if (Python3_NumPy_FOUND)
+#    target_link_libraries(matplotlib_cpp INTERFACE
+#            Python3::NumPy
+#    )
+#else ()
+message(WARNING "NumPy not found. Matplotlib-cpp will be built without NumPy support.")
+target_compile_definitions(matplotlib_cpp INTERFACE WITHOUT_NUMPY)
+#endif ()

firmware/hexray/FSM/src/io/io_apps.cpp

@@ -2,6 +2,7 @@
 #include "hw_adcs.hpp"
 #include "util_utils.hpp"
 #include <cmath>
+#include <algorithm>
 
 //=====
 // Geometry and ADC Constants for Pedal Sensors -> PAPPS = primary apps, SAPPS = secondary apps (same as liam's
@@ -71,7 +72,7 @@ static float calcAppsAngle(const float cos_law_coefficient, const float pot_len,
     // Calculate the cosine law expression: (a^2 + b^2 - c^2) / (2ab)
     const float value = cos_law_coefficient - pot_len * pot_len / cos_law_denominator;
     const float acos_input =
-        CLAMP(value, -1.0f, 1.0f); // where c is represented indirectly via the measured length (pot_len)
+        std::clamp(value, -1.0f, 1.0f); // where c is represented indirectly via the measured length (pot_len)
     return acos(acos_input);
 }
 
@@ -110,7 +111,7 @@ float getPrimary(void)
 
     // Scale the percentage to account for the dead zone.
     const float pedal_percentage = (100.0f / (100.0f - DEAD_ZONE_PERCENT)) * (pedal_percentage_raw - DEAD_ZONE_PERCENT);
-    return CLAMP(pedal_percentage, 0.0f, 100.0f);
+    return std::clamp(pedal_percentage, 0.0f, 100.0f);
 }
 
 bool isPrimaryOCSC(void)
@@ -139,7 +140,7 @@ float getSecondary(void)
 
     // Scale the percentage to account for the dead zone.
     const float pedal_percentage = (100.0f / (100.0f - DEAD_ZONE_PERCENT)) * (pedal_percentage_raw - DEAD_ZONE_PERCENT);
-    return CLAMP(pedal_percentage, 0.0f, 100.0f);
+    return std::clamp(pedal_percentage, 0.0f, 100.0f);
 }
 
 bool isSecondaryOCSC(void)

firmware/hexray/VC/CMakeLists.txt

@@ -16,7 +16,9 @@ set(SYSTEM_INCLUDE_DIRS
 )
 
 file(GLOB_RECURSE APP_SRCS CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/app/*.cpp")
-list(APPEND APP_SRCS)
+list(APPEND APP_SRCS
+        "${SHARED_APP_INCLUDE_DIR_CPP}/app_pid.cpp"
+)
 set(APP_INCLUDE_DIRS
         "${SHARED_APP_INCLUDE_DIR}" "${SHARED_APP_INCLUDE_DIR_CPP}" "${CMAKE_CURRENT_SOURCE_DIR}/src/app/"
 )
@@ -128,7 +130,7 @@ if ("${TARGET}" STREQUAL "binary")
             "${CMAKE_CURRENT_BINARY_DIR}/app"
     )
 
-    target_link_libraries("hexray_VC_app.elf" PRIVATE "hexray_VC_stm32" "hexray_VC_commit_info" "hexray_VC_jsoncan" "m" "sbg_ecom_${ARM_CORE}")
+    target_link_libraries("hexray_VC_app.elf" PRIVATE "hexray_VC_stm32" "hexray_VC_commit_info" "hexray_VC_jsoncan" "m" "autodiff_interface" "sbg_ecom_${ARM_CORE}")
     target_compile_definitions("hexray_VC_app.elf" PRIVATE VC)
 
     add_chimera_stm32h7("hexray_VC_chimera" "${CHIMERA_SRCS}" "${CHIMERA_INCLUDE_DIRS}" "hexray_chimera_v2_proto_cm7")
@@ -143,7 +145,8 @@ elseif ("${TARGET}" STREQUAL "test")
     )
     set(INCLUDE_DIRS
             "${CMAKE_CURRENT_SOURCE_DIR}/test"
-            ${APP_INCLUDE_DIRS} ${SHARED_FAKES_DIR_CPP} ${IO_INCLUDE_DIRS} ${TEST_INCLUDE_DIRS} ${SYSTEM_INCLUDE_DIRS} ${SHARED_UTIL_INCLUDE_DIR_CPP}
+            ${APP_INCLUDE_DIRS} ${SHARED_FAKES_DIR_CPP} ${IO_INCLUDE_DIRS} ${TEST_INCLUDE_DIRS}
+            ${SYSTEM_INCLUDE_DIRS} ${SHARED_UTIL_INCLUDE_DIR_CPP}
     )
     compile_gtest_executable(
             "hexray_VC_test"
@@ -163,6 +166,33 @@ elseif ("${TARGET}" STREQUAL "test")
             "${CMAKE_CURRENT_BINARY_DIR}/app"
     )
 
-    target_link_libraries("hexray_VC_test" PRIVATE "hexray_VC_commit_info" "hexray_VC_jsoncan")
+    target_link_libraries("hexray_VC_test" PRIVATE "hexray_VC_commit_info" "hexray_VC_jsoncan" "autodiff_interface")
     target_compile_definitions("hexray_VC_test" PRIVATE STM32H733xx)
+
+
+    # MATLAB STATIC LIBRARY
+    file(GLOB_RECURSE TV_SRCS CONFIGURE_DEPENDS
+            "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/*.cpp"
+            "${SHARED_APP_INCLUDE_DIR_CPP}/app_pid.cpp"
+    )
+    add_library("hexray_VC_torque_vectoring" STATIC ${TV_SRCS})
+    target_include_directories("hexray_VC_torque_vectoring" PUBLIC "${INCLUDE_DIRS}")
+    target_link_libraries("hexray_VC_torque_vectoring" PUBLIC "autodiff_interface" "eigen_interface" "m")
+    target_compile_options("hexray_VC_torque_vectoring" PRIVATE /MT /bigobj)
+
+
+    # MATPLOTLIB TESTING
+    add_executable(test_controls)
+    target_sources(test_controls PRIVATE
+            "${CMAKE_CURRENT_SOURCE_DIR}/plot_tests.cpp"
+            "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/estimation/tire_model.cpp"
+            "${CMAKE_CURRENT_SOURCE_DIR}/src/app/torque_vectoring/controllers/torque_allocator.cpp"
+    )
+    target_include_directories(test_controls PRIVATE
+            "${CMAKE_CURRENT_SOURCE_DIR}/src/app"
+            ${SHARED_UTIL_INCLUDE_DIR_CPP}
+    )
+    target_link_libraries(test_controls PRIVATE "matplotlib_cpp" "autodiff_interface")
+    target_compile_options(test_controls PRIVATE /Zi)
+    target_link_options(test_controls PRIVATE /DEBUG /INCREMENTAL:NO)
 endif ()

firmware/hexray/VC/plot_tests.cpp

@@ -0,0 +1,102 @@
+// ReSharper disable CppTooWideScope
+#include "matplotlibcpp.h"
+#include "torque_vectoring/estimation/tire_model.hpp"
+#include "torque_vectoring/controllers/torque_allocator.hpp"
+
+#include <vector>
+#include <string>
+#include <iomanip>
+
+namespace plt = matplotlibcpp;
+
+static void plot_combined_fx()
+{
+    constexpr int   n_points = 301; // includes endpoints
+    constexpr float fz_N     = 700.0f;
+
+    std::vector<double> kappas;
+    kappas.reserve(n_points);
+    for (int i = 0; i < n_points; ++i)
+    {
+        constexpr double kappa_max = 0.6f;
+        constexpr double kappa_min = -0.6f;
+        const double     t         = static_cast<float>(i) / static_cast<float>(n_points - 1);
+        const double     kappa     = kappa_min + t * (kappa_max - kappa_min);
+        kappas.push_back(kappa);
+    }
+
+    for (const float alpha_rad : { 0.15f, 0.125f, 0.1f, 0.075f, 0.05f, 0.025f, 0.0f })
+    {
+        std::vector<double> fxs;
+        fxs.reserve(kappas.size());
+        for (const double kappa : kappas)
+        {
+            fxs.push_back(app::tv::estimation::tire_model.computeCombinedFx_N<double>(fz_N, alpha_rad, kappa));
+        }
+        std::stringstream ss;
+        ss << "\\alpha = " << std::setprecision(2) << alpha_rad;
+        plt::named_plot(ss.str(), kappas, fxs);
+    }
+
+    plt::title("F_x as a function of \\kappa and \\alpha");
+    plt::xlabel("\\kappa");
+    plt::ylabel("F_x (N)");
+    plt::grid(true);
+    plt::legend();
+    plt::show();
+}
+
+static void plot_combined_fy()
+{
+    constexpr int   n_points = 301; // includes endpoints
+    constexpr float fz_N     = 700.0f;
+
+    std::vector<float> alphas;
+    alphas.reserve(n_points);
+    for (int i = 0; i < n_points; ++i)
+    {
+        constexpr float alpha_max = 0.6f;
+        constexpr float alpha_min = -0.6f;
+        const float     t         = static_cast<float>(i) / static_cast<float>(n_points - 1);
+        const float     alpha     = alpha_min + t * (alpha_max - alpha_min);
+        alphas.push_back(alpha);
+    }
+
+    for (const double kappa : { 0.3, 0.25, 0.2, 0.15, 0.1, 0.05, 0.0 })
+    {
+        std::vector<double> fys;
+        fys.reserve(alphas.size());
+        for (const float alpha : alphas)
+        {
+            fys.push_back(app::tv::estimation::tire_model.computeCombinedFy_N<double>(fz_N, alpha, kappa));
+        }
+        std::stringstream ss;
+        ss << "\\kappa = " << std::setprecision(2) << kappa;
+        plt::named_plot(ss.str(), alphas, fys);
+    }
+
+    plt::title("F_y as a function of \\alpha and \\kappa");
+    plt::xlabel("\\alpha");
+    plt::ylabel("F_y (N)");
+    plt::grid(true);
+    plt::legend();
+    plt::show();
+}
+
+int main()
+{
+    // constexpr app::tv::shared_datatypes::VehicleState<double> state{
+    //     .v_x_mps        = 10,
+    //     .v_y_mps        = 0,
+    //     .yaw_rate_radps = 0,
+    //     .a_x_mps2       = 1,
+    //     .a_y_mps2       = 0,
+    //     .apps           = 0.2,
+    //     .delta          = 0,
+    // };
+    // const auto [fl, fr, rl, rr] = app::tv::controllers::allocator::optimize(state, 10.0, 5.0);
+    // std::cout << "Optimal slip found: " << fl << " " << fr << " " << rl << " " << rr << std::endl;
+
+    // plot_combined_fx();
+    plot_combined_fy();
+}

firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_config.hpp

@@ -1,3 +1,4 @@
+#pragma once
 #include "app_pid.hpp"
 
 namespace app::tv::controllers::dyrc

firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_dyrc.cpp

@@ -0,0 +1,40 @@
+#include "controllers_dyrc.hpp"
+#include "torque_vectoring/controllers/controllers_config.hpp"
+#include "torque_vectoring/shared_datatypes/constants.hpp"
+
+using namespace app::tv::shared_datatypes::vd_constants;
+
+namespace app::tv::controllers::dyrc
+{
+
+// Configuration
+static float ku = DYRC_ku; // understeer gradient
+
+// Control Scheme
+static PID pid(PID_DYRC_config);
+
+// purely for debugging purposes
+static float yaw_moment_Nm = 0.0f;
+static float r_ref_rad     = 0.0f;
+
+[[nodiscard]] float computeRefYawRate(const float steer_ang_rad, const float body_velx_mps)
+{
+    return (body_velx_mps * steer_ang_rad) / (WHEELBASE_m * (1.0f + ku * body_velx_mps * body_velx_mps));
+}
+
+[[nodiscard]] float computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps)
+{
+    r_ref_rad            = computeRefYawRate(steer_ang_rad, body_velx_mps);
+    return yaw_moment_Nm = pid.compute(r_ref_rad, r_actual_rad, 0.0f);
+}
+
+[[nodiscard]] float getYawMoment()
+{
+    return yaw_moment_Nm;
+}
+
+[[nodiscard]] float getRefYawRate()
+{
+    return r_ref_rad;
+}
+} // namespace app::tv::controllers::dyrc

firmware/hexray/VC/src/app/torque_vectoring/controllers/yaw_rate_control/controllers_dyrc.hpp → firmware/hexray/VC/src/app/torque_vectoring/controllers/controllers_dyrc.hpp

@@ -21,7 +21,7 @@ namespace app::tv::controllers::dyrc
  *
  * @return The reference yaw rate to target in rad/s
  */
-[[nodiscard]] inline float computeRefYawRate(const float steer_ang_rad, const float body_velx_mps);
+[[nodiscard]] float computeRefYawRate(const float steer_ang_rad, const float body_velx_mps);
 
 /**
  * @brief Computes the corrective yaw moment to apply on the vehicle to target a reference yaw rate
@@ -34,8 +34,7 @@ namespace app::tv::controllers::dyrc
  *
  * @return The corrective yaw moment in Nm to apply on the vehicle
  */
-[[nodiscard]] inline float
-    computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps);
+[[nodiscard]] float computeYawMoment(const float r_actual_rad, const float steer_ang_rad, const float body_velx_mps);
 
 // The functions below are getters for CAN debugging
 
@@ -52,4 +51,4 @@ namespace app::tv::controllers::dyrc
  * @return The desired yaw rate to achieve in radians per second
  */
 [[nodiscard]] float getRefYawRate();
-} // namespace app::tv::controllers::dyrc
+} // namespace app::tv::controllers::dyrc