Simp vd interface update function

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;
 }

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;
 };