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