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..12ddcde05 --- /dev/null +++ b/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; +} \ 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..7913599c9 --- /dev/null +++ b/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; +}; \ 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 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..7feded2ee --- /dev/null +++ b/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 +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, 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 \ 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