From 0f55fe916d7cb978dd7f6549799491b1e8277265 Mon Sep 17 00:00:00 2001 From: Tegh25 Date: Sun, 24 Nov 2024 00:05:40 -0600 Subject: [PATCH 1/4] Basic vd interface class implementation and inout --- .../FrontController/inc/simp_vd_interface.cc | 11 +++++++ .../FrontController/inc/simp_vd_interface.h | 32 +++++++++++++++++++ .../inc/simp_vd_interface_test.cc | 1 + 3 files changed, 44 insertions(+) create mode 100644 firmware/projects/FrontController/inc/simp_vd_interface.cc create mode 100644 firmware/projects/FrontController/inc/simp_vd_interface.h create mode 100644 firmware/projects/FrontController/inc/simp_vd_interface_test.cc diff --git a/firmware/projects/FrontController/inc/simp_vd_interface.cc b/firmware/projects/FrontController/inc/simp_vd_interface.cc new file mode 100644 index 000000000..df858cea9 --- /dev/null +++ b/firmware/projects/FrontController/inc/simp_vd_interface.cc @@ -0,0 +1,11 @@ +#include "simp_vd_interface.h" + +SimpVdInterface::SimpVdInterface() { + +} + +VdOutput SimpVdInterface::update(const VdInput& input, int time_ms) { + VdOutput output; + + return output; +} \ No newline at end of file diff --git a/firmware/projects/FrontController/inc/simp_vd_interface.h b/firmware/projects/FrontController/inc/simp_vd_interface.h new file mode 100644 index 000000000..e31734172 --- /dev/null +++ b/firmware/projects/FrontController/inc/simp_vd_interface.h @@ -0,0 +1,32 @@ +#pragma once + +#include "app.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(); + VdOutput update(const VdInput& input, int time_ms); + +private: + +}; \ No newline at end of file diff --git a/firmware/projects/FrontController/inc/simp_vd_interface_test.cc b/firmware/projects/FrontController/inc/simp_vd_interface_test.cc new file mode 100644 index 000000000..1559ac08a --- /dev/null +++ b/firmware/projects/FrontController/inc/simp_vd_interface_test.cc @@ -0,0 +1 @@ +// tests for the simp_vd_interface class \ No newline at end of file From 8d119d4e77b2a3acc8c2e0e06439f6e52bc729e1 Mon Sep 17 00:00:00 2001 From: Tegh25 Date: Thu, 5 Dec 2024 23:45:42 -0600 Subject: [PATCH 2/4] Shared controls includes for vd interface --- firmware/shared/controls/motor_torque.h | 64 ++++++++++++++++++++++ firmware/shared/controls/tc_scale_factor.h | 29 ++++++++++ firmware/shared/controls/tvFactor.h | 44 +++++++++++++++ 3 files changed, 137 insertions(+) create mode 100644 firmware/shared/controls/motor_torque.h create mode 100644 firmware/shared/controls/tc_scale_factor.h create mode 100644 firmware/shared/controls/tvFactor.h diff --git a/firmware/shared/controls/motor_torque.h b/firmware/shared/controls/motor_torque.h new file mode 100644 index 000000000..9c54702b6 --- /dev/null +++ b/firmware/shared/controls/motor_torque.h @@ -0,0 +1,64 @@ +#pragma once +#include + +#include "shared/util/moving_average.h" + +// Peter Jabra and Aleeza Ali Zar +namespace ctrl { + +enum class State { + Run, + Stop, +}; + +template +std::tuple CalculateMotorTorque(T new_torque_value, T right_factor, + T left_factor, bool reset = false) { + static shared::util::MovingAverage running_average; + + if (reset) { + running_average = shared::util::MovingAverage(); + } + + 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 +T ComputeTorqueRequest(T driver_torque_request, T brake_pedal_position) { + static State current_state = State::Stop; + bool brake_on = brake_pedal_position > static_cast(10); + + bool both_pedals_pressed = + (driver_torque_request >= static_cast(25) && brake_on); + bool neither_pedal_pressed = + driver_torque_request < static_cast(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(0.0); + break; + } + + return driver_torque; +} + +} // namespace ctrl \ No newline at end of file diff --git a/firmware/shared/controls/tc_scale_factor.h b/firmware/shared/controls/tc_scale_factor.h new file mode 100644 index 000000000..27cbe9baa --- /dev/null +++ b/firmware/shared/controls/tc_scale_factor.h @@ -0,0 +1,29 @@ +#pragma once + +namespace ctrl { +// Note: The CalculateActualSlip function has Div-by-Zero error if left front +// and right front wheel speeds = 0. +template +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 +T CalculateTCScaleFactor(T actual_slip, T target_slip) { + // Stateflow: Multi-Stage TC + return 0; +} +} // namespace ctrl \ No newline at end of file diff --git a/firmware/shared/controls/tvFactor.h b/firmware/shared/controls/tvFactor.h new file mode 100644 index 000000000..09a738bc0 --- /dev/null +++ b/firmware/shared/controls/tvFactor.h @@ -0,0 +1,44 @@ +#pragma once +#include +#include +#include "../util/mappers/lookup_table.h" + +namespace ctrl{ + +template +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 tv_lookup_table{table_data}; + + return tv_lookup_table.Evaluate(absolute_steering_angle); +} + +template +std::tuple 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(1); + right_torque_vector = torque_vectoring_factor; + } else if (steering_angle < 0) { + right_torque_vector = static_cast(1); + left_torque_vector = torque_vectoring_factor; + } else { + left_torque_vector = static_cast(1); + right_torque_vector = static_cast(1); + } + + return std::tuple(left_torque_vector, right_torque_vector); +} +} \ No newline at end of file From 0b5cd85fb9fbbeb9b484c48a57f5c3f84c1e1c97 Mon Sep 17 00:00:00 2001 From: Tegh25 Date: Fri, 6 Dec 2024 18:04:21 -0600 Subject: [PATCH 3/4] Calculate TC scale factor function --- firmware/shared/controls/tc_scale_factor.h | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/firmware/shared/controls/tc_scale_factor.h b/firmware/shared/controls/tc_scale_factor.h index 27cbe9baa..7feded2ee 100644 --- a/firmware/shared/controls/tc_scale_factor.h +++ b/firmware/shared/controls/tc_scale_factor.h @@ -22,8 +22,23 @@ T CalculateActualSlip(T left_rear_wheel_speed, T right_rear_wheel_speed, } template -T CalculateTCScaleFactor(T actual_slip, T target_slip) { +T CalculateTCScaleFactor(T actual_slip, T target_slip, int time_ms) { // Stateflow: Multi-Stage TC - return 0; + 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 \ No newline at end of file From c19d6bc79b05e7b8edf7dbb6c0bf63418bc660d3 Mon Sep 17 00:00:00 2001 From: Tegh25 Date: Thu, 12 Dec 2024 20:06:11 -0600 Subject: [PATCH 4/4] Simp vd interface update function --- .../FrontController/inc/simp_vd_interface.cc | 32 ++++++++++++++++--- .../FrontController/inc/simp_vd_interface.h | 16 ++++++++-- 2 files changed, 42 insertions(+), 6 deletions(-) diff --git a/firmware/projects/FrontController/inc/simp_vd_interface.cc b/firmware/projects/FrontController/inc/simp_vd_interface.cc index df858cea9..12ddcde05 100644 --- a/firmware/projects/FrontController/inc/simp_vd_interface.cc +++ b/firmware/projects/FrontController/inc/simp_vd_interface.cc @@ -1,11 +1,35 @@ +/// @author Teghveer Singh Ateliey +/// @date 2024-11-23 + #include "simp_vd_interface.h" -SimpVdInterface::SimpVdInterface() { - -} +using namespace ctrl; + +SimpVdInterface::SimpVdInterface(float target_slip) + : target_slip(target_slip) {} VdOutput SimpVdInterface::update(const VdInput& input, int time_ms) { - VdOutput output; + 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; } \ No newline at end of file diff --git a/firmware/projects/FrontController/inc/simp_vd_interface.h b/firmware/projects/FrontController/inc/simp_vd_interface.h index e31734172..7913599c9 100644 --- a/firmware/projects/FrontController/inc/simp_vd_interface.h +++ b/firmware/projects/FrontController/inc/simp_vd_interface.h @@ -1,6 +1,12 @@ +/// @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; @@ -24,9 +30,15 @@ struct VdOutput { class SimpVdInterface { public: - SimpVdInterface(); + 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; }; \ No newline at end of file