Merge pull request #38 from KSU-MS/newlaunchlookupmode

Adding SlipTime based TC control similar to megasquirt

include/tc_system.h

@@ -9,7 +9,7 @@
 class torque_control_system
 {
 public:
-    torque_control_system(std::initializer_list<torque_control_types_e> enabledTypes = {torque_control_types_e::TC_DRIVERCONTROL, torque_control_types_e::TC_PID})
+    torque_control_system(std::initializer_list<torque_control_types_e> enabledTypes = {torque_control_types_e::TC_DRIVERCONTROL, torque_control_types_e::TC_PID, torque_control_types_e::TC_SlipTime})
     {
         for (auto type : enabledTypes)
         {
@@ -34,11 +34,13 @@ class torque_control_system
     torque_control_types_e tcType = torque_control_types_e::TC_DRIVERCONTROL;
     torque_controller tc_base;
     torque_controllerPID tc_pid = torque_controllerPID(1.0, 0, 0);
+    torque_controllerSlipTime tc_sliptime;
 
     // This map should contain ALL types
     std::unordered_map<torque_control_types_e, void *> tc_map = {
         {torque_control_types_e::TC_DRIVERCONTROL, &tc_base},
-        {torque_control_types_e::TC_PID, &tc_pid}};
+        {torque_control_types_e::TC_PID, &tc_pid},
+        {torque_control_types_e::TC_SlipTime, &tc_sliptime}};
 
     // This map will only have enabled types
     std::unordered_map<torque_control_types_e, void *> enabled_tc_map;

include/torque_controller.hpp

@@ -12,6 +12,7 @@ enum torque_control_types_e
 {
     TC_DRIVERCONTROL = 0, // No firmware limiting, driver throttle directly
     TC_PID = 1, // PID slip control
+    TC_SlipTime = 2, // Time slip control
     TC_NUM_CONTROLLERS
 };
 
@@ -96,4 +97,22 @@ class torque_controllerPID : public torque_controller
         pid.setGains(d_kp,d_ki,d_kd);
     }
 };
+
+class torque_controllerSlipTime : public torque_controller
+{
+private:
+    const double tireSlipHigh = 0.15; // Slip threshold
+    unsigned long _lastSlip = 0; // Time slip began
+    unsigned long slip_dT = 0; // Time delta since slip first began
+    double slipTime = 0; // Slip * time, the x axis
+    bool slipActive = false; // Flag to indicate if slip is active
+    static const int numPoints = 6; // Number of data points
+    double yTorqueRTD[numPoints] = {20, 30, 40, 50, 80, 120}; // TQ in Nm to reduce the output by
+    double xSlipTime[numPoints] = {100, 200, 300, 500, 750, 1000}; // SlipTime, is % slip * time (ms) 
+    double slopes[numPoints]; // Slopes at each point for interpolation
+    double outputTorqueRTD = 0; // Torque Retard due to controller
+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_SlipTime;}
+};
 #endif

src/tc_system.cpp

@@ -39,6 +39,11 @@ void torque_control_system::printControllerType()
             printf("Controller type is tc_PID");
             break;
         }
+        case (torque_control_types_e::TC_SlipTime):
+        {
+            printf("Controller type is tc_SlipTime");
+            break;
+        }
         case (torque_control_types_e::TC_NUM_CONTROLLERS):
         {
             printf("? LOL");

src/torque_controller.cpp

@@ -54,3 +54,72 @@ int16_t torque_controllerPID::calculate_torque(unsigned long elapsedTime, int16_
     outputTorqueCommand = static_cast<int16_t>(torqueOut); // Post-clamping
     return outputTorqueCommand;  
 }
+
+// SlipTime based torque reduction
+int16_t torque_controllerSlipTime::calculate_torque(unsigned long elapsedTime, int16_t maxTorque, wheelSpeeds_s &wheelSpeedData)
+{
+    driverTorqueRequest = maxTorque;
+    float torqueOut = 0;
+    float slipRatio = 0;
+
+    // Calculate front and rear wheel speeds - take average of left and right
+    float frontRpmAvg = ((wheelSpeedData.fl+wheelSpeedData.fr)/2);
+    float rearRpmAvg =  ((wheelSpeedData.rl+wheelSpeedData.rr)/2);
+    printf("TC Front avg: %f Rear avg: %f\n",frontRpmAvg,rearRpmAvg);
+    printf("TC WHEELSPEEDS \nFL: %f FR: %f\nRL: %f RR: %f\n",wheelSpeedData.fl,wheelSpeedData.fr,wheelSpeedData.rl,wheelSpeedData.rr);
+    // avoid zero division
+    if (frontRpmAvg || rearRpmAvg <= 0.001)
+    {
+        slipRatio = 0; // treat it like 0 slip (maybe this is bad)
+    }
+    else
+    {
+        // Slip = (rear / front) - 1
+        // ie. 1000rpm/900rpm = 1.111..
+        // 1.111 - 1 = 0.111 slip ratio
+        slipRatio = (rearRpmAvg / frontRpmAvg) - 1;
+    }
+
+    // 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]) {
+        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];
+    }
+
+    // Limit torque outputs like normal
+    torqueOut = driverTorqueRequest - (outputTorqueRTD * 10); // times 10 because your input is coming from the output of calculate_torque based on the APPS which is Nmx10, and subtracts the amount of torque you want to remove from the driver's request
+    lcTorqueRequest = static_cast<int16_t>(torqueOut); // Pre clamping
+    if (torqueOut > maxTorque)
+    {
+        torqueOut = maxTorque;
+    }
+    if (torqueOut < 0)
+    {
+        torqueOut = 0;
+    }
+
+    outputTorqueCommand = static_cast<int16_t>(torqueOut); // Post-clamping
+    return outputTorqueCommand;  
+}