From cb3eb79d01792dcd4705dd64eaaf5317afca04d0 Mon Sep 17 00:00:00 2001
From: jstri114 <jstri114@students.kennesaw.edu>
Date: Wed, 29 May 2024 17:03:04 -0400
Subject: [PATCH] more bad code

---
 include/torque_controller.hpp | 28 ++++++++++++++----------
 src/torque_controller.cpp     | 41 ++++++++++++++++++++++++++---------
 2 files changed, 48 insertions(+), 21 deletions(-)

diff --git a/include/torque_controller.hpp b/include/torque_controller.hpp
index 433513c..2d028f2 100644
--- a/include/torque_controller.hpp
+++ b/include/torque_controller.hpp
@@ -12,7 +12,7 @@ enum torque_control_types_e
 {
     TC_DRIVERCONTROL = 0, // No firmware limiting, driver throttle directly
     TC_PID = 1, // PID slip control
-    TC_TimeSlip = 2, // Time slip control
+    TC_SlipTime = 2, // Time slip control
     TC_NUM_CONTROLLERS
 };
 
@@ -98,19 +98,25 @@ class torque_controllerPID : public torque_controller
     }
 };
 
-class torque_controllerTimeSlip : public torque_controller
+class torque_controllerSlipTime : public torque_controller
 {
 private:
-    const double tireSlipHigh = 0.5;
-    double d_kp = 1.0;
-    const double output_min = -1.0; // Minimum output of the PID controller
-    const double output_max = 0; // Max output of the PID controller
-    double input, setpoint, output;
-    unsigned long _lastStep = 0; // Last time step???
-    unsigned long _dT = 0; // Time since last update
-    const uint8_t TC_PID_TIMESTEP = 5; //ms
+    const double tireSlipHigh = 0.05; // Slip threshold
+    unsigned long _lastSlip = 0; // Time slip began
+    unsigned long slip_dT = 0; // Time delta since slip first began
+    bool slipActive = false; // Flag to indicate if slip is active
+    static const int numPoints = 5; // Number of data points
+    double xSlipTime[numPoints] = {10, 20, 30, 40, 50}; // x-coordinates of data points
+    double yTorqueRTD[numPoints] = {0.0, 0.5, 0.6, 0.2, 0.0}; // y-coordinates of data points
+    double slopes[numPoints]; // Slopes at each point for interpolation
+    double outputTorqueRTD = 0; // Torque Retard due to controller
+    double slipTime = 0;
+
+
 public:
     int16_t calculate_torque(unsigned long elapsedTime, int16_t maxTorque, wheelSpeeds_s &wheelSpeedData);
-    torque_control_types_e getType() {return torque_control_types_e::TC_TimeSlip;}
+    torque_control_types_e getType() {return torque_control_types_e::TC_SlipTime;}
+
+   
 };
 #endif
\ No newline at end of file
diff --git a/src/torque_controller.cpp b/src/torque_controller.cpp
index 456e3a0..9c3ea04 100644
--- a/src/torque_controller.cpp
+++ b/src/torque_controller.cpp
@@ -55,8 +55,8 @@ int16_t torque_controllerPID::calculate_torque(unsigned long elapsedTime, int16_
     return outputTorqueCommand;  
 }
 
-// TimeSlip based torque reduction
-int16_t torque_controllerTimeSlip::calculate_torque(unsigned long elapsedTime, int16_t maxTorque, wheelSpeeds_s &wheelSpeedData)
+// SlipTime based torque reduction
+int16_t torque_controllerSlipTime::calculate_torque(unsigned long elapsedTime, int16_t maxTorque, wheelSpeeds_s &wheelSpeedData)
 {
     driverTorqueRequest = maxTorque;
     float torqueOut = 0;
@@ -80,13 +80,37 @@ int16_t torque_controllerTimeSlip::calculate_torque(unsigned long elapsedTime, i
         slipRatio = (rearRpmAvg / frontRpmAvg) - 1;
     }
  
-    //Calculate time since last update
-    _dT = elapsedTime - _lastStep;   //calculate time since last update
-
+    // Set the "state" of the controller to capture the time the slip ratio crossed the threshold (which is used to calculate slip * time)
+    if (slipRatio > tireSlipHigh && slipActive == false) { // If the slip ratio crosses the threshold, set the last slip time to now and set the slip active flag to true
+        _lastSlip = elapsedTime;
+        slipActive = true;
+    } else if (slipRatio > tireSlipHigh && slipActive == true) { // If slip ratio above threshold and slip active, calculate time since beginning to slip and slip * time
+        slip_dT = elapsedTime - _lastSlip; // Calculate time since slip started
+        slipTime = (slipRatio * slip_dT); // Calculate slip * time
+    } else {
+        slipActive = false; // Catch all, should disable whenever slip ratio falls below threshold and slip active was true
+    }
+   
 
+    // Check bounds
+    if (slipTime < xSlipTime[0] || slipTime > xSlipTime[numPoints - 1]) {
+        Serial.println("slipTime out of bounds");
+        outputTorqueRTD = 0;
+    }
 
+    // Find the interval where 'x' belongs
+    for (int i = 0; i < numPoints - 1; i++) {
+        if (slipTime <= xSlipTime[i + 1]) {
+            double slope = (yTorqueRTD[i + 1] - yTorqueRTD[i]) / (xSlipTime[i + 1] - xSlipTime[i]); // Calculate slope for linear interpolation
+            outputTorqueRTD = yTorqueRTD[i] + slope * (slipTime - xSlipTime[i]); // Calculate output based on linear interpolation
+        }
+    }
+    // If slipTime lands on the last point, return the last value
+    if (slipTime = xSlipTime[numPoints - 1]) {
+        outputTorqueRTD = yTorqueRTD[numPoints - 1];
+    }
     
-    torqueOut = maxTorque + (output * maxTorque);
+    torqueOut = outputTorqueRTD;
     lcTorqueRequest = static_cast<int16_t>(torqueOut); // Pre clamping
     if (torqueOut > maxTorque)
     {
@@ -97,10 +121,7 @@ int16_t torque_controllerTimeSlip::calculate_torque(unsigned long elapsedTime, i
         torqueOut = 0;
     }
 
-
-
+    
     outputTorqueCommand = static_cast<int16_t>(torqueOut); // Post-clamping
-
-    _lastStep = elapsedTime; // Set current time as last time for the future calculation
     return outputTorqueCommand;  
 }
\ No newline at end of file