Implement the Simple Vehicle Dynamics Interface in C++

firmware/projects/FrontController/inc/simp_vd_interface.cc

@@ -0,0 +1,35 @@
+/// @author Teghveer Singh Ateliey
+/// @date 2024-11-23
+
+#include "simp_vd_interface.h"
+
+using namespace ctrl;
+
+SimpVdInterface::SimpVdInterface(float target_slip)
+    : target_slip(target_slip) {}
+
+VdOutput SimpVdInterface::update(const VdInput& input, int time_ms) {
+    VdOutput output{
+        .lm_torque_limit_negative = 0.0f,
+        .rm_torque_limit_negative = 0.0f,
+        .left_motor_speed_request = 1000,
+        .right_motor_speed_request = 1000
+    };
+
+    motor_torque_request = ComputeTorqueRequest(input.driver_torque_request,
+                                                input.brake_pedal_postion);
+    actual_slip =
+        CalculateActualSlip(input.wheel_speed_lr, input.wheel_speed_rr,
+                            input.wheel_speed_lf, input.wheel_speed_rf);
+    tc_scale_factor = CalculateTCScaleFactor(actual_slip, target_slip, time_ms);
+
+    float steering_angle = input.tv_enable ? input.steering_angle : 0.0f;
+    std::tie(tv_factor_left, tv_factor_right) = AdjustTorqueVectoring(
+        steering_angle, CreateTorqueVectoringFactor(steering_angle));
+
+    std::tie(output.lm_torque_limit_positive, output.rm_torque_limit_positive) =
+        CalculateMotorTorque(motor_torque_request * tc_scale_factor,
+                             tv_factor_right, tv_factor_left);
+
+    return output;
+}

firmware/projects/FrontController/inc/simp_vd_interface.h

@@ -0,0 +1,44 @@
+/// @author Teghveer Singh Ateliey
+/// @date 2024-11-23
+
+#pragma once
+
+#include "app.h"
+#include "shared/controls/motor_torque.h"
+#include "shared/controls/tc_scale_factor.h"
+#include "shared/controls/tvFactor.h"
+
+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(
+        float target_slip = 0.2f);  // default target slip is float 0.2
+    VdOutput update(const VdInput& input, int time_ms);
+
+private:
+    float target_slip;
+    float motor_torque_request;
+    float actual_slip;
+    float tc_scale_factor;
+    float tv_factor_left;
+    float tv_factor_right;
+};

firmware/projects/FrontController/inc/simp_vd_interface_test.cc

@@ -0,0 +1 @@
+// tests for the simp_vd_interface class

firmware/shared/controls/motor_torque.h

@@ -0,0 +1,64 @@
+#pragma once
+#include <tuple>
+
+#include "shared/util/moving_average.h"
+
+// Peter Jabra and Aleeza Ali Zar
+namespace ctrl {
+
+enum class State {
+    Run,
+    Stop,
+};
+
+template <typename T>
+std::tuple<T, T> CalculateMotorTorque(T new_torque_value, T right_factor,
+                                      T left_factor, bool reset = false) {
+    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();
+
+    T right_motor_torque_limit = running_average_value * right_factor;
+    T left_motor_torque_limit = running_average_value * left_factor;
+
+    return std::tuple(right_motor_torque_limit, left_motor_torque_limit);
+}
+
+template <typename T>
+T ComputeTorqueRequest(T driver_torque_request, T brake_pedal_position) {
+    static State current_state = State::Stop;
+    bool brake_on = brake_pedal_position > static_cast<T>(10);
+
+    bool both_pedals_pressed =
+        (driver_torque_request >= static_cast<T>(25) && brake_on);
+    bool neither_pedal_pressed =
+        driver_torque_request < static_cast<T>(5) && !brake_on;
+
+    if (both_pedals_pressed) {
+        current_state = State::Stop;
+    }
+    if (neither_pedal_pressed) {
+        current_state = State::Run;
+    }
+
+    T driver_torque;
+
+    switch (current_state) {
+        case State::Run:
+            driver_torque = driver_torque_request;
+            break;
+        case State::Stop:
+            driver_torque = static_cast<T>(0.0);
+            break;
+    }
+
+    return driver_torque;
+}
+
+}  // namespace ctrl

firmware/shared/controls/tc_scale_factor.h

@@ -0,0 +1,44 @@
+#pragma once
+
+namespace ctrl {
+// Note: The CalculateActualSlip function has Div-by-Zero error if left front
+// and right front wheel speeds = 0.
+template <typename T>
+T CalculateActualSlip(T left_rear_wheel_speed, T right_rear_wheel_speed,
+                      T left_front_wheel_speed, T right_front_wheel_speed) {
+    T idle_wheel_spd = (left_front_wheel_speed + right_front_wheel_speed) / 2.0;
+    T actual_slip;
+
+    if (left_rear_wheel_speed > right_rear_wheel_speed) {
+        actual_slip = (left_rear_wheel_speed / idle_wheel_spd) - 1;
+    } else {
+        actual_slip = (right_rear_wheel_speed / idle_wheel_spd) - 1;
+    }
+
+    if (actual_slip < 0) {
+        actual_slip = 0;
+    }
+    return actual_slip;
+}
+
+template <typename T>
+T CalculateTCScaleFactor(T actual_slip, T target_slip, int time_ms) {
+    // Stateflow: Multi-Stage TC
+    T scale_factor;
+
+    if (time_ms > 50 && time_ms <= 83 && actual_slip > target_slip) {
+        scale_factor = 0.0f;
+    } 
+    else if (time_ms > 83 && time_ms <= 116 && actual_slip <= target_slip) {
+        scale_factor = 0.25f;
+    }
+    else if (time_ms > 116 && time_ms <= 149) {
+        scale_factor = 0.5f;
+    }
+    else {
+        scale_factor = 1.0f;
+    }
+
+    return scale_factor;
+}
+}  // namespace ctrl

firmware/shared/controls/tvFactor.h

@@ -0,0 +1,44 @@
+#pragma once
+#include <algorithm>
+#include <tuple>
+#include "../util/mappers/lookup_table.h"
+
+namespace ctrl{
+
+template <typename T>
+T CreateTorqueVectoringFactor(T steering_angle) {
+    T absolute_steering_angle = std::abs(steering_angle);
+
+    static const float table_data[][2] = {
+        {0.0, 1.0f},    {5.0, 0.934f},  {10.0, 0.87f},
+        {15.0, 0.808f}, {20.0, 0.747f}, {25.0, 0.683f},
+    };
+
+    static constexpr int table_length =
+        (sizeof(table_data)) / (sizeof(table_data[0]));
+
+    static shared::util::LookupTable<table_length> tv_lookup_table{table_data};
+
+    return tv_lookup_table.Evaluate(absolute_steering_angle);
+}
+
+template <typename T>
+std::tuple<T, T> AdjustTorqueVectoring(T steering_angle,
+                                       T torque_vectoring_factor) {
+    T left_torque_vector;
+    T right_torque_vector;
+
+    if (steering_angle > 0) {
+        left_torque_vector = static_cast<T>(1);
+        right_torque_vector = torque_vectoring_factor;
+    } else if (steering_angle < 0) {
+        right_torque_vector = static_cast<T>(1);
+        left_torque_vector = torque_vectoring_factor;
+    } else {
+        left_torque_vector = static_cast<T>(1);
+        right_torque_vector = static_cast<T>(1);
+    }
+
+    return std::tuple(left_torque_vector, right_torque_vector);
+}
+}